int led_pwm_servo_init_group_map(uint8_t timer_map, uint32_t channel_map) { if (timer_map == 0 || channel_map == 0) { return -EINVAL; } uint8_t tim_idx = 0; while (((timer_map >> tim_idx) & 1) == 0) { tim_idx ++; } while (((1 << tim_idx) & timer_map) != 0) { io_timer_init_timer(tim_idx); tim_idx ++; } /* uint8_t ch_idx = 0; while ((channel_map & 1) == 0) { channel_map >>= 1; } while (((1 << ch_idx) & channel_map) != 0) { led_pwm_channel_init(ch_idx); ch_idx ++; } */ up_pwm_servo_init(channel_map, 0); return OK; }
HighSpeedPWM(unsigned int pwm_rate) : _pwm_rate(pwm_rate), _pwm_period(1.0f / (float) pwm_rate) { /* There's some sleeps in here that might not all be necessary... */ printf("Resetting GPIO\n"); gpioReset(); usleep(500000); /* Magic number borrowed from src/drivers/px4fmu/fmu.cpp */ printf("Initializing PWM\n"); up_pwm_servo_init(0x3f); usleep(500000); /* Set PWM Rate for all outputs */ printf("Setting PWM rate\n"); if(up_pwm_servo_set_rate(_pwm_rate) != OK) { printf("set rate failed"); return; } usleep(500000); printf("Arming PWM\n"); up_pwm_servo_arm(true); usleep(500000); }
void CameraInterfacePWM::setup() { for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]); up_pwm_servo_init(pin_bitmask); up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); } } }
int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ up_cxxinitialize(); /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* default to 50 Hz PWM outputs */ system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); /* print some startup info */ lib_lowprintf("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); /* turn on servo power */ POWER_SERVO(true); /* start the safety switch handler */ safety_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); /* start the flight control signal handler */ task_create("FCon", SCHED_PRIORITY_DEFAULT, 1024, (main_t)controls_main, NULL); struct mallinfo minfo = mallinfo(); lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); /* we're done here, go run the communications loop */ comms_main(); }
int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ up_cxxinitialize(); /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); /* calculate our fw CRC so FMU can decide if we need to update */ calculate_fw_crc(); /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ #ifdef CONFIG_ARCH_DMA hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* print some startup info */ lowsyslog("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); #ifdef GPIO_LED4 LED_RING(false); #endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO POWER_SERVO(true); #endif /* turn off S.Bus out (if supported) */ #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(false); #endif /* start the safety switch handler */ safety_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); /* initialise the control inputs */ controls_init(); /* set up the ADC */ adc_init(); /* start the FMU interface */ interface_init(); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* add a performance counter for controls */ perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); /* * P O L I C E L I G H T S * * Not enough memory, lock down. * * We might need to allocate mixers later, and this will * ensure that a developer doing a change will notice * that he just burned the remaining RAM with static * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. * */ if (minfo.mxordblk < 600) { lowsyslog("ERR: not enough MEM"); bool phase = false; while (true) { if (phase) { LED_AMBER(true); LED_BLUE(false); } else { LED_AMBER(false); LED_BLUE(true); } up_udelay(250000); phase = !phase; } } /* Start the failsafe led init */ failsafe_led_init(); /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); controls_tick(); perf_end(controls_perf); if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) { last_heartbeat_time = hrt_absolute_time(); heartbeat_blink(); } ring_blink(); check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); /* post debug state at ~1Hz - this is via an auxiliary serial port * DEFAULTS TO OFF! */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } }
int PX4FMU::set_mode(Mode mode) { /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always * listening and mixing; the mode just selects which of the channels * are presented on the output pins. */ switch (mode) { case MODE_2PWM: // v1 multi-port with flow control lines as PWM debug("MODE_2PWM"); /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; /* XXX magic numbers */ up_pwm_servo_init(0x3); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; case MODE_4PWM: // v1 multi-port as 4 PWM outs debug("MODE_4PWM"); /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; /* XXX magic numbers */ up_pwm_servo_init(0xf); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; case MODE_6PWM: // v2 PWMs as 6 PWM outs debug("MODE_6PWM"); /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; /* XXX magic numbers */ up_pwm_servo_init(0x3f); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; #ifdef CONFIG_ARCH_BOARD_AEROCORE case MODE_8PWM: // AeroCore PWMs as 8 PWM outs debug("MODE_8PWM"); /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; /* XXX magic numbers */ up_pwm_servo_init(0xff); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; #endif case MODE_NONE: debug("MODE_NONE"); _pwm_default_rate = 10; /* artificially reduced output rate */ _pwm_alt_rate = 10; _pwm_alt_rate_channels = 0; /* disable servo outputs - no need to set rates */ up_pwm_servo_deinit(); break; default: return -EINVAL; } _mode = mode; return OK; }
int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ up_cxxinitialize(); /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ #ifdef CONFIG_ARCH_DMA hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* print some startup info */ lowsyslog("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); /* turn on servo power */ POWER_SERVO(true); /* start the safety switch handler */ safety_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); /* initialise the control inputs */ controls_init(); #ifdef CONFIG_STM32_I2C1 /* start the i2c handler */ i2c_init(); #endif /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* add a performance counter for controls */ perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); #if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); bool phase = false; if (phase) { LED_AMBER(true); LED_BLUE(false); } else { LED_AMBER(false); LED_BLUE(true); } phase = !phase; usleep(300000); } #endif /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; for (;;) { /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); controls_tick(); perf_end(controls_perf); /* check for debug activity */ show_debug_messages(); /* post debug state at ~1Hz */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } } }
int user_start(int argc, char *argv[]) { /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) /* run C++ ctors before we go any further */ up_cxxinitialize(); # if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) # error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. # endif #else # error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. #endif /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); /* calculate our fw CRC so FMU can decide if we need to update */ calculate_fw_crc(); /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ #ifdef CONFIG_ARCH_DMA hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* print some startup info */ syslog(LOG_INFO, "\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); #ifdef GPIO_LED4 LED_RING(false); #endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO POWER_SERVO(true); #endif /* turn off S.Bus out (if supported) */ #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(false); #endif /* start the safety switch handler */ safety_init(); /* initialise the control inputs */ controls_init(); /* set up the ADC */ adc_init(); /* start the FMU interface */ interface_init(); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* add a performance counter for controls */ perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk; syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); /* * P O L I C E L I G H T S * * Not enough memory, lock down. * * We might need to allocate mixers later, and this will * ensure that a developer doing a change will notice * that he just burned the remaining RAM with static * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. * */ if (minfo.mxordblk < 600) { syslog(LOG_ERR, "ERR: not enough MEM"); bool phase = false; while (true) { if (phase) { LED_AMBER(true); LED_BLUE(false); } else { LED_AMBER(false); LED_BLUE(true); } up_udelay(250000); phase = !phase; } } /* Start the failsafe led init */ failsafe_led_init(); /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; uint64_t last_heartbeat_time = 0; uint64_t last_loop_time = 0; for (;;) { dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f; last_loop_time = hrt_absolute_time(); if (dt < 0.0001f) { dt = 0.0001f; } else if (dt > 0.02f) { dt = 0.02f; } /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); controls_tick(); perf_end(controls_perf); /* some boards such as Pixhawk 2.1 made the unfortunate choice to combine the blue led channel with the IMU heater. We need a software hack to fix the hardware hack by allowing to disable the LED / heater. */ if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) { /* blink blue LED at 4Hz in normal operation. When in override blink 4x faster so the user can clearly see that override is happening. This helps when pre-flight testing the override system */ uint32_t heartbeat_period_us = 250 * 1000UL; if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { heartbeat_period_us /= 4; } if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) { last_heartbeat_time = hrt_absolute_time(); heartbeat_blink(); } } else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) { /* switch resistive heater off */ LED_BLUE(false); } else { /* switch resistive heater hard on */ LED_BLUE(true); } update_mem_usage(); ring_blink(); check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); /* post debug state at ~1Hz - this is via an auxiliary serial port * DEFAULTS TO OFF! */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } }
void FMUServo::task_main() { /* configure for PWM output */ switch (_mode) { case MODE_2PWM: /* multi-port with flow control lines as PWM */ /* XXX magic numbers */ up_pwm_servo_init(0x3); break; case MODE_4PWM: /* multi-port as 4 PWM outs */ /* XXX magic numbers */ up_pwm_servo_init(0xf); break; case MODE_NONE: /* we should never get here... */ break; } /* subscribe to objects that we are interested in watching */ _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(_t_actuators, 20); /* 50Hz update rate */ _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 100); /* 10Hz update rate */ struct pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_armed; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; log("starting"); /* loop until killed */ while (!_task_should_exit) { /* sleep waiting for data, but no more than 100ms */ int ret = ::poll(&fds[0], 2, 1000); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); usleep(1000000); continue; } /* do we have a control update? */ if (fds[0].revents & POLLIN) { float outputs[num_outputs]; /* get controls - must always do this to avoid spinning */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { /* do mixing */ _mixers->mix(&outputs[0], num_outputs); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { /* scale for PWM output 900 - 2100us */ up_pwm_servo_set(i, 1500 + (600 * outputs[i])); } } } /* how about an arming update? */ if (fds[1].revents & POLLIN) { struct actuator_armed_s aa; /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); /* update PMW servo armed status */ up_pwm_servo_arm(aa.armed); } } ::close(_t_actuators); ::close(_t_armed); /* make sure servos are off */ up_pwm_servo_deinit(); log("stopping"); /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ _task = -1; _exit(0); }