예제 #1
0
static void
heartbeat_blink(void)
{
	static bool heartbeat = false;
	LED_BLUE(heartbeat = !heartbeat);
#ifdef GPIO_LED4
	LED_RING(heartbeat);
#endif
}
예제 #2
0
파일: px4io.c 프로젝트: NikiRegina/Firmware
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();
}
예제 #3
0
파일: px4io.c 프로젝트: PX4-Works/Firmware
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();
		}
	}
}
예제 #4
0
파일: px4io.c 프로젝트: PX4-Works/Firmware
static void
heartbeat_blink(void)
{
	static bool heartbeat = false;
	LED_BLUE(heartbeat = !heartbeat);
}
예제 #5
0
int
user_start(int argc, char *argv[])
{
	/* run C++ ctors before we go any further */
	up_cxxinitialize();

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

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

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

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

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

	/* turn on servo power */
	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();
		}
	}
}
예제 #6
0
파일: px4io.c 프로젝트: Aerovinci/Firmware
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();
		}
	}
}
예제 #7
0
int main(void)
{
	Init();

	DBG_MSG("----- Power On -----");

	if(USBDevice_PlugIn())
	{
		DBG_MSG( "Usb Init Started");
		USB_Init();
	}else{
		DBG_MSG("FileSystem_Init");
		FileSystem_Init();
		fileTest();
	}

	while(true);//Stop here

	char color[] = {0xff, 0xff, 0x00};
	WS2812_Set(0, 3, color);
	color[0] = 0x00;
	color[2] = 0xff;
	WS2812_Set(3, 3, color);
	color[0] = 0xff;
	color[1] = 0x00;
	WS2812_Set(6, 3, color);

	LED_RED(true);
	Delay_ms(200);
	LED_GREEN(true);
	Delay_ms(200);
	LED_BLUE(true);
	

	DBG_MSG("Temperature: %f", TMP102_GetTemp());
	DBG_MSG("Temperature: %f", TMP102_GetTemp());

	// WavePlayer_Init();
	// WavePlayerMenu_Start("/", "teq.wav");
	// WavePlayer_Start();

	// Reflective_Start();

	// Analog_SetChannel(PHOTOTRANS_1_CH, true);
	// Analog_SetChannel(PHOTOTRANS_2_CH, true);
	// Analog_SetChannel(PHOTOTRANS_3_CH, true);
	// Analog_SetChannel(PHOTOTRANS_4_CH, true);
	// Analog_SetChannel(PHOTOTRANS_5_CH, true);

	Delay_ms(2000);

	MPU9250_InitProcedure();

	SysTick_t tick = 0;
	while(true) {
		float accel[3], gyro[3], mag[3];
		float yaw, pitch, roll;


		if(GetSystemTick() - tick > 1000){
			if(MPU9250_CheckNewSample()){
				MPU9250_Get9AxisData(accel, gyro, mag);
				MPU9250_CalcOrientation(&yaw, &pitch, &roll);
			}
			DBG_MSG("Temperature: %f", TMP102_GetTemp());

			// DBG_MSG("ADC: %d %d %d %d %d",
			// 	Analog_GetChannelValue(PHOTOTRANS_1_CH),
			// 	Analog_GetChannelValue(PHOTOTRANS_2_CH),
			// 	Analog_GetChannelValue(PHOTOTRANS_3_CH),
			// 	Analog_GetChannelValue(PHOTOTRANS_4_CH),
			// 	Analog_GetChannelValue(PHOTOTRANS_5_CH)
			// );
			for (int i = 0; i < 3; ++i)
			{
				DBG_MSG("MPU-Accel-%c: %f", i+'X', accel[i]);
			}
			for (int i = 0; i < 3; ++i)
			{
				DBG_MSG("MPU-Gyro-%c: %f", i+'X', gyro[i]);
			}
			for (int i = 0; i < 3; ++i)
			{
				DBG_MSG("MPU-Mag-%c: %f", i+'X', mag[i]);
			}
			DBG_MSG("MPU-Temp: %f", MPU9250_GetTemperature());
			DBG_MSG("yaw: %f, pitch: %f, roll: %f", yaw, pitch, roll);
			tick = GetSystemTick();
		}
	}
}