int UavcanBarometerBridge::init() { int res = device::CDev::init(); if (res < 0) { return res; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { return -1; } res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } return 0; }
int PX4FLOW::init() { int ret = PX4_ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) { return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; if (_class_instance == CLASS_DEVICE_PRIMARY) { _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } else { DEVICE_LOG("not primary range device, not advertising"); } ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; /* get yaw rotation from sensor frame to body frame */ param_t rot = param_find("SENS_FLOW_ROT"); /* only set it if the parameter exists */ if (rot != PARAM_INVALID) { int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward param_get(rot, &val); _sensor_rotation = (enum Rotation)val; } return ret; }
int PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY) { int ret; uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0, DIR_READ(0x05), 0, DIR_READ(0x06), 0 }; ret = transfer(&data[0], &data[0], 10); if (OK != ret) { perf_count(_comms_errors); DEVICE_LOG("spi::transfer returned %d", ret); return ret; } deltaX = ((int16_t)data[5] << 8) | data[3]; deltaY = ((int16_t)data[9] << 8) | data[7]; ret = OK; return ret; }
/* Wrapper to read a byte from addr */ int PCA9685::read8(uint8_t addr, uint8_t &value) { int ret = OK; /* send addr */ ret = transfer(&addr, sizeof(addr), nullptr, 0); if (ret != OK) { goto fail_read; } /* get value */ ret = transfer(nullptr, 0, &value, 1); if (ret != OK) { goto fail_read; } return ret; fail_read: perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); return ret; }
uint16_t ADC::_sample(unsigned channel) { perf_begin(_sample_perf); /* clear any previous EOC */ if (rSR & ADC_SR_EOC) { rSR &= ~ADC_SR_EOC; } /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; rCR2 |= ADC_CR2_SWSTART; /* wait for the conversion to complete */ hrt_abstime now = hrt_absolute_time(); while (!(rSR & ADC_SR_EOC)) { /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 50) { DEVICE_LOG("sample timeout"); return 0xffff; } } /* read the result and clear EOC */ uint16_t result = rDR; perf_end(_sample_perf); return result; }
void TRONE::cycle() { /* collection phase? */ if (_collect_phase) { /* perform collection */ if (OK != collect()) { DEVICE_LOG("collection error"); /* restart the measurement state machine */ start(); return; } /* next phase is measurement */ _collect_phase = false; /* * Is there a collect->measure gap? */ if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); return; } } /* measurement phase */ if (OK != measure()) { DEVICE_LOG("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)&TRONE::cycle_trampoline, this, USEC2TICK(TRONE_CONVERSION_INTERVAL)); }
int TRONE::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[3] = {0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 3); if (ret < 0) { DEVICE_LOG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } uint16_t distance_mm = (val[0] << 8) | val[1]; float distance_m = float(distance_mm) * 1e-3f; struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); /* there is no enum item for a combined LASER and ULTRASOUND which it should be */ report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.orientation = 8; report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; /* TODO: set proper ID */ report.id = 0; // This validation check can be used later _valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance && (float)report.current_distance < report.max_distance ? 1 : 0; /* publish it, if we are the primary */ if (_distance_sensor_topic != nullptr) { orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
int UavcanBarometerBridge::init() { int res = device::CDev::init(); if (res < 0) { return res; } res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } return 0; }
/* Wrapper to wite a byte to addr */ int PCA9685::write8(uint8_t addr, uint8_t value) { int ret = OK; _msg[0] = addr; _msg[1] = value; /* send addr and value */ ret = transfer(_msg, 2, nullptr, 0); if (ret != OK) { perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); } return ret; }
void TAP_ESC::work_stop() { for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_unsubscribe(_control_subs[i]); _control_subs[i] = -1; } } orb_unsubscribe(_armed_sub); _armed_sub = -1; orb_unsubscribe(_test_motor_sub); _test_motor_sub = -1; DEVICE_LOG("stopping"); _initialized = false; }
int UavcanMagnetometerBridge::init() { int res = device::CDev::init(); if (res < 0) { return res; } res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } return 0; }
int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case BAROIOCSMSLPRESSURE: { if ((arg < 80000) || (arg > 120000)) { return -EINVAL; } else { DEVICE_LOG("new msl pressure %u", _msl_pressure); _msl_pressure = arg; return OK; } } case BAROIOCGMSLPRESSURE: { return _msl_pressure; } case SENSORIOCSPOLLRATE: { // not supported yet, pretend that everything is ok return OK; } 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; } default: { return CDev::ioctl(filp, cmd, arg); } } }
int SPI::init() { int ret = OK; /* attach to the spi bus */ if (_dev == nullptr) { _dev = up_spiinitialize(_bus); } if (_dev == nullptr) { DEVICE_DEBUG("failed to init SPI"); ret = -ENOENT; goto out; } // tell other SPI users that we may be doing transfers in // interrupt context up_spi_set_need_irq_save(_dev); /* deselect device to ensure high to low transition of pin select */ SPI_SELECT(_dev, _device, false); /* call the probe function to check whether the device is present */ ret = probe(); if (ret != OK) { DEVICE_DEBUG("probe failed"); goto out; } /* do base class init, which will create the device node, etc. */ ret = CDev::init(); if (ret != OK) { DEVICE_DEBUG("cdev init failed"); goto out; } /* tell the workd where we are */ DEVICE_LOG("on SPI bus %d at %d (%u KHz)", _bus, _device, _frequency / 1000); out: return ret; }
int SF0X::init() { /* status */ int ret = 0; do { /* create a scope to handle exit conditions using break */ /* do regular cdev init */ ret = CDev::init(); if (ret != OK) { break; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { warnx("mem err"); ret = -1; break; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } } while (0); /* close the fd */ ::close(_fd); _fd = -1; return OK; }
int PMW3901::writeRegister(unsigned reg, uint8_t data) { uint8_t cmd[2]; // write 1 byte int ret; cmd[0] = DIR_WRITE(reg); cmd[1] = data; ret = transfer(&cmd[0], nullptr, 2); if (OK != ret) { perf_count(_comms_errors); DEVICE_LOG("spi::transfer returned %d", ret); return ret; } return ret; }
int SF0X::measure() { int ret; /* * Send the command to begin a measurement. */ char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); if (ret != sizeof(cmd)) { perf_count(_comms_errors); DEVICE_LOG("write fail %d", ret); return ret; } ret = OK; return ret; }
int PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count) { uint8_t cmd[5]; // read up to 4 bytes int ret; cmd[0] = DIR_READ(reg); ret = transfer(&cmd[0], &cmd[0], count + 1); if (OK != ret) { perf_count(_comms_errors); DEVICE_LOG("spi::transfer returned %d", ret); return ret; } memcpy(&data[0], &cmd[1], count); return ret; }
int TRONE::measure() { int ret; /* * Send the command to begin a measurement. */ const uint8_t cmd = TRONE_MEASURE_REG; ret = transfer(&cmd, sizeof(cmd), nullptr, 0); if (OK != ret) { perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } ret = OK; return ret; }
int PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) { int ret; /* convert to correct message */ _msg[0] = LED0_ON_L + 4 * num; _msg[1] = on; _msg[2] = on >> 8; _msg[3] = off; _msg[4] = off >> 8; /* try i2c transfer */ ret = transfer(_msg, 5, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); } return ret; }
int TRONE::init() { int ret = ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) { goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report; measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; out: return ret; }
void PX4FMU::work_stop() { xTimerStop(_work, portMAX_DELAY); for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } } ::close(_armed_sub); //::close(_param_sub); /* make sure servos are off */ up_pwm_servo_deinit(); DEVICE_LOG("stopping\n"); /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ _initialized = false; }
int ADC::init() { /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_CAL; usleep(100); if (rCR2 & ADC_CR2_CAL) { return -1; } #endif /* arbitrarily configure all channels for 55 cycle sample time */ rSMPR1 = 0b00000011011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; /* XXX for F2/4, might want to select 12-bit mode? */ rCR1 = 0; /* enable the temperature sensor / Vrefint channel if supported*/ rCR2 = #ifdef ADC_CR2_TSVREFE /* enable the temperature sensor in CR2 */ ADC_CR2_TSVREFE | #endif 0; #ifdef ADC_CCR_TSVREFE /* enable temperature sensor in CCR */ rCCR = ADC_CCR_TSVREFE; #endif /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; usleep(10); rCR2 |= ADC_CR2_ADON; usleep(10); rCR2 |= ADC_CR2_ADON; usleep(10); /* kick off a sample and wait for it to complete */ hrt_abstime now = hrt_absolute_time(); rCR2 |= ADC_CR2_SWSTART; while (!(rSR & ADC_SR_EOC)) { /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 500) { DEVICE_LOG("sample timeout"); return -1; } } DEVICE_DEBUG("init done"); /* create the device node */ return CDev::init(); }
int SF1XX::init() { int ret = PX4_ERROR; int hw_model; param_get(param_find("SENS_EN_SF1XX"), &hw_model); switch (hw_model) { case 0: DEVICE_LOG("disabled."); return ret; case 1: /* SF10/a (25m 32Hz) */ _min_distance = 0.01f; _max_distance = 25.0f; _conversion_interval = 31250; break; case 2: /* SF10/b (50m 32Hz) */ _min_distance = 0.01f; _max_distance = 50.0f; _conversion_interval = 31250; break; case 3: /* SF10/c (100m 16Hz) */ _min_distance = 0.01f; _max_distance = 100.0f; _conversion_interval = 62500; break; case 4: /* SF11/c (120m 20Hz) */ _min_distance = 0.01f; _max_distance = 120.0f; _conversion_interval = 50000; break; case 5: /* SF20/LW20 (100m 48-388Hz) */ _min_distance = 0.001f; _max_distance = 100.0f; _conversion_interval = 20834; break; default: DEVICE_LOG("invalid HW model %d.", hw_model); return ret; } /* do I2C init (and probe) first */ if (I2C::init() != OK) { return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); set_address(SF1XX_BASEADDR); if (_reports == nullptr) { return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } // Select altitude register int ret2 = measure(); if (ret2 == 0) { ret = OK; _sensor_ok = true; DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance, (int)(1e6f / _conversion_interval), SF1XX_BASEADDR); } return ret; }
int MB12XX::init() { int ret = PX4_ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) { return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } // XXX we should find out why we need to wait 200 ms here usleep(200000); /* check for connected rangefinders on each i2c port: We start from i2c base address (0x70 = 112) and count downwards So second iteration it uses i2c address 111, third iteration 110 and so on*/ for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ addr_ind.push_back(_index_counter); DEVICE_DEBUG("sonar added"); _latest_sonar_measurements.push_back(200); } } _index_counter = MB12XX_BASEADDR; set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */ /* if only one sonar detected, no special timing is required between firing, so use default */ if (addr_ind.size() == 1) { _cycling_rate = MB12XX_CONVERSION_INTERVAL; } else { _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; } /* show the connected sonars in terminal */ for (unsigned i = 0; i < addr_ind.size(); i++) { DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]); } DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size()); ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; return ret; }
void PX4FMU::task_main() { /* force a reset of the update rate */ _current_update_rate = 0; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _param_sub = orb_subscribe(ORB_ID(parameter_update)); #ifdef HRT_PPM_CHANNEL // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; #endif /* initialize PWM limit lib */ pwm_limit_init(&_pwm_limit); update_pwm_rev_mask(); /* loop until killed */ while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; /* force setting update rate */ _current_update_rate = 0; } /* * Adjust actuator topic update rate to keep up with * the highest servo update rate configured. * * We always mix at max rate; some channels may update slower. */ unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; if (_current_update_rate != max_rate) { _current_update_rate = max_rate; int update_rate_in_ms = int(1000 / _current_update_rate); /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { update_rate_in_ms = 2; } /* reject slower than 10 Hz updates */ if (update_rate_in_ms > 100) { update_rate_in_ms = 100; } DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); } } // set to current max rate, even if we are actually checking slower/faster _current_update_rate = max_rate; } /* sleep waiting for data, stopping to check for PPM * input at 50Hz */ int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { DEVICE_LOG("poll error %d", errno); continue; } else if (ret == 0) { /* timeout: no control data, switch to failsafe values */ // warnx("no PWM: failsafe"); } else { /* get controls for required topics */ unsigned poll_id = 0; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } poll_id++; } } /* can we mix? */ if (_mixers != nullptr) { size_t num_outputs; switch (_mode) { case MODE_2PWM: num_outputs = 2; break; case MODE_4PWM: num_outputs = 4; break; case MODE_6PWM: num_outputs = 6; break; case MODE_8PWM: num_outputs = 8; break; default: num_outputs = 0; break; } /* do mixing */ float outputs[_max_actuators]; num_outputs = _mixers->mix(outputs, num_outputs, NULL); /* disable unused ports by setting their output to NaN */ for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { if (i >= num_outputs) { outputs[i] = NAN_VALUE; } } uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* output to the servos */ for (size_t i = 0; i < num_outputs; i++) { up_pwm_servo_set(i, pwm_limited[i]); } publish_pwm_outputs(pwm_limited, num_outputs); } } /* check arming state */ bool updated = false; orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown; if (_servo_armed != set_armed) { _servo_armed = set_armed; } /* update PWM status if armed or if disarmed PWM values are set */ bool pwm_on = (set_armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; up_pwm_servo_arm(pwm_on); } } orb_check(_param_sub, &updated); if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); update_pwm_rev_mask(); } #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp_last_signal) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { rc_in.channel_count = RC_INPUT_MAX_CHANNELS; } for (uint8_t i = 0; i < rc_in.channel_count; i++) { rc_in.values[i] = ppm_buffer[i]; } rc_in.timestamp_publication = ppm_last_valid_decode; rc_in.timestamp_last_signal = ppm_last_valid_decode; rc_in.rc_ppm_frame_length = ppm_frame_length; rc_in.rssi = RC_INPUT_RSSI_MAX; rc_in.rc_failsafe = false; rc_in.rc_lost = false; rc_in.rc_lost_frame_count = 0; rc_in.rc_total_frame_count = 0; /* lazily advertise on first publication */ if (to_input_rc == 0) { to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); } else { orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); } } #endif } for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } } ::close(_armed_sub); ::close(_param_sub); /* make sure servos are off */ up_pwm_servo_deinit(); DEVICE_LOG("stopping"); /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ _task = -1; _exit(0); }
/** * Main loop function */ void PCA9685::i2cpwm() { if (_mode == IOX_MODE_TEST_OUT) { setPin(0, PCA9685_PWMCENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { if (!_mode_on_initialized) { // Init PWM limits pwm_limit_init(&_pwm_limit); // Get arming state _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* Subscribe to actuator groups */ subscribe(); /* set the uorb update interval lower than the driver pwm interval */ for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) { orb_set_interval(_control_subs[i], 1000.0f / PCA9685_PWMFREQ - 5); } _mode_on_initialized = true; } /* check if anything updated */ int ret = ::poll(_poll_fds, _poll_fds_num, 0); if (ret < 0) { DEVICE_LOG("poll error %d", errno); } else if (ret == 0) { // warnx("no PWM: failsafe"); } else { /* get controls for required topics */ unsigned poll_id = 0; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } } poll_id++; } if (_mixers != nullptr) { size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; // do mixing num_outputs = _mixers->mix(_outputs, num_outputs, NULL); // disable unused ports by setting their output to NaN for (size_t i = 0; i < sizeof(_outputs) / sizeof(_outputs[0]); i++) { if (i >= num_outputs) { _outputs[i] = NAN_VALUE; } } // Finally, write servo values to motors for (int i = 0; i < num_outputs; i++) { uint16_t new_value = PCA9685_PWMCENTER + (_outputs[i] * M_PI_F * PCA9685_SCALE); // DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, // (double)_controls[1].control[i]); if (isfinite(new_value) && new_value >= PCA9685_PWMMIN && new_value <= PCA9685_PWMMAX) { setPin(i, new_value); _rates[i] = new_value; } } } } bool updated; // Update Arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown; if (_servo_armed != set_armed) { _servo_armed = set_armed; } } // Update AUX controls update // orb_check(_actuator_controls_sub, &updated); // if (updated) { // size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; // // // Get updated actuator // // Only update actuator 1 for now // orb_copy(ORB_ID(actuator_controls_1), _actuator_controls_sub, &_controls[1]); // // // } _should_run = true; } // check if any activity remains, else stop if (!_should_run) { _running = false; return; } // re-queue ourselves to run again later _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval); }
int I2C::init() { int ret = OK; unsigned bus_index; // attach to the i2c bus _dev = px4_i2cbus_initialize(_bus); if (_dev == nullptr) { DEVICE_DEBUG("failed to init I2C"); ret = -ENOENT; goto out; } // the above call fails for a non-existing bus index, // so the index math here is safe. bus_index = _bus - 1; // abort if the max frequency we allow (the frequency we ask) // is smaller than the bus frequency if (_bus_clocks[bus_index] > _frequency) { (void)px4_i2cbus_uninitialize(_dev); _dev = nullptr; DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); ret = -EINVAL; goto out; } // set frequency for this instance once to the bus speed // the bus speed is the maximum supported by all devices on the bus, // as we have to prioritize performance over compatibility. // If a new device requires a lower clock speed, this has to be // manually set via "fmu i2c <bus> <clock>" before starting any // drivers. // This is necessary as automatically lowering the bus speed // for maximum compatibility could induce timing issues on // critical sensors the adopter might be unaware of. // set the bus frequency on the first access if it has // not been set yet if (_bus_clocks[bus_index] == 0) { _bus_clocks[bus_index] = _frequency; } // call the probe function to check whether the device is present ret = probe(); if (ret != OK) { DEVICE_DEBUG("probe failed"); goto out; } // do base class init, which will create device node, etc ret = CDev::init(); if (ret != OK) { DEVICE_DEBUG("cdev init failed"); goto out; } // tell the world where we are DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); out: if ((ret != OK) && (_dev != nullptr)) { px4_i2cbus_uninitialize(_dev); _dev = nullptr; } return ret; }
int SF0X::init() { int hw_model; param_get(param_find("SENS_EN_SF0X"), &hw_model); switch (hw_model) { case 0: DEVICE_LOG("disabled."); return 0; case 1: /* SF02 (40m, 12 Hz)*/ _min_distance = 0.3f; _max_distance = 40.0f; _conversion_interval = 83334; break; case 2: /* SF10/a (25m 32Hz) */ _min_distance = 0.01f; _max_distance = 25.0f; _conversion_interval = 31250; break; case 3: /* SF10/b (50m 32Hz) */ _min_distance = 0.01f; _max_distance = 50.0f; _conversion_interval = 31250; break; case 4: /* SF10/c (100m 16Hz) */ _min_distance = 0.01f; _max_distance = 100.0f; _conversion_interval = 62500; break; case 5: /* SF11/c (120m 20Hz) */ _min_distance = 0.01f; _max_distance = 120.0f; _conversion_interval = 50000; break; default: DEVICE_LOG("invalid HW model %d.", hw_model); return -1; } /* status */ int ret = 0; do { /* create a scope to handle exit conditions using break */ /* do regular cdev init */ ret = CDev::init(); if (ret != OK) { break; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { warnx("mem err"); ret = -1; break; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } while (0); /* close the fd */ ::close(_fd); _fd = -1; return OK; }
int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); // XXX figure out the output count _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); memset(&_outputs, 0, sizeof(_outputs)); /* * Set up the time synchronization */ const int slave_init_res = _time_sync_slave.start(); if (slave_init_res < 0) { warnx("Failed to start time_sync_slave"); _task_should_exit = true; } /* When we have a system wide notion of time update (i.e the transition from the initial * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that * happens, but for now we use adjustUtc with a correction of the hrt so that the * time bases are the same */ uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time())); _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } /* * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); * IO multiplexing shall be done here. */ _node.setModeOperational(); /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). * Please note that with such multiplexing it is no longer possible to rely only on * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). */ add_poll_fd(busevent_fd); /* * setup poll to look for actuator direct input if we are * subscribed to the topic */ if (_actuator_direct_sub != -1) { _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } while (!_task_should_exit) { switch (_fw_server_action) { case Start: _fw_server_status = start_fw_server(); break; case Stop: _fw_server_status = stop_fw_server(); break; case CheckFW: _fw_server_status = request_fw_check(); break; case None: default: break; } // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; } // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); perf_begin(_perfcnt_esc_mixer_total_elapsed); (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking bool new_output = false; // this would be bad... if (poll_ret < 0) { DEVICE_LOG("poll error %d", errno); continue; } else { // get controls for required topics bool controls_updated = false; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } } } /* see if we have any direct actuator updates */ if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && !_test_in_progress) { if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } memcpy(&_outputs.output[0], &_actuator_direct.values[0], _actuator_direct.nvalues * sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; } // can we mix? if (_test_in_progress) { memset(&_outputs, 0, sizeof(_outputs)); if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; _outputs.noutputs = _test_motor.motor_number + 1; } new_output = true; } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. unsigned num_outputs_max = 8; // Do mixing _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL); new_output = true; } } if (new_output) { // iterate actuators, checking for valid values for (uint8_t i = 0; i < _outputs.noutputs; i++) { // last resort: catch NaN, INF and out-of-band errors if (!isfinite(_outputs.output[i])) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally * spinning motors. It would be deadly in flight. */ _outputs.output[i] = -1.0f; } // never go below min if (_outputs.output[i] < -1.0f) { _outputs.output[i] = -1.0f; } // never go above max if (_outputs.output[i] > 1.0f) { _outputs.output[i] = 1.0f; } } // Output to the bus _outputs.timestamp = hrt_absolute_time(); perf_begin(_perfcnt_esc_mixer_output_elapsed); _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); perf_end(_perfcnt_esc_mixer_output_elapsed); } // Check motor test state bool updated = false; orb_check(_test_motor_sub, &updated); if (updated) { orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); // Update the test status and check that we're not locked down _test_in_progress = (_test_motor.value > 0); _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); } // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); // Update the armed status and check that we're not locked down and motor // test is not running bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } } (void)::close(busevent_fd); teardown(); warnx("exiting."); exit(0); }
void SF0X::cycle() { /* fds initialized? */ if (_fd < 0) { /* open fd */ _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); } /* 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) { 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)&SF0X::cycle_trampoline, this, _measure_ticks - USEC2TICK(_conversion_interval)); return; } } /* measurement phase */ if (OK != measure()) { DEVICE_LOG("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)); }