Пример #1
0
__task void serial_process()
{
    UART_Configuration config;
    int32_t len_data = 0;
    void *msg;

    while (1) {
        // Check our mailbox to see if we need to set anything up with the UART
        // before we do any sending or receiving
        if (os_mbx_wait(&serial_mailbox, &msg, 0) == OS_R_OK) {
            switch((SERIAL_MSG)(unsigned)msg) {
                case SERIAL_INITIALIZE:
                    uart_initialize();
                    break;

                case SERIAL_UNINITIALIZE:
                    uart_uninitialize();
                    break;

                case SERIAL_RESET:
                    uart_reset();
                    break;

                case SERIAL_SET_CONFIGURATION:
                    serial_get_configuration(&config);
                    uart_set_configuration(&config);
                    break;

                default:
                    break;
            }
        }

        len_data = USBD_CDC_ACM_DataFree();
        if (len_data > SIZE_DATA) {
            len_data = SIZE_DATA;
        }
        if (len_data) {
            len_data = uart_read_data(data, len_data);
        }
        if (len_data) {
            if(USBD_CDC_ACM_DataSend(data , len_data)) {
                main_blink_cdc_led(MAIN_LED_OFF);
            }
        }

        len_data = uart_write_free();
        if (len_data > SIZE_DATA) {
            len_data = SIZE_DATA;
        }
        if (len_data) {
            len_data = USBD_CDC_ACM_DataRead(data, len_data);
        }
        if (len_data) {
            if (uart_write_data(data, len_data)) {
                main_blink_cdc_led(MAIN_LED_OFF);
            }
        }
    }
}
Пример #2
0
Файл: main.c Проект: 8l/potato
int main(void)
{
	const char * hello_string = "Hello world\n\r";

	uart_initialize(&uart0, (volatile void *) PLATFORM_UART0_BASE);
	uart_set_divisor(&uart0, uart_baud2divisor(115200, PLATFORM_SYSCLK_FREQ));

	for(int i = 0; hello_string[i] != 0; ++i)
	{
		while(uart_tx_fifo_full(&uart0));
		uart_tx(&uart0, hello_string[i]);
	}

	return 0;
}
Пример #3
0
//console thread.
void console() {
	char* r;
	
	//basic initialization of needed hw components.
	uart_initialize();
	mmc_initialize();
	utfs_open(&current_directory, "/");
	
	writeString("Hello!\r\n");
	for (;;) {
		writeString("> ");
		r = readString();
		writeString("\r\n");
		runcmd(r);
	}
}
Пример #4
0
int main(void)
{
  // Stop watchdog timer for now
  WDTCTL = WDTPW | WDTHOLD;

  // Enable JTAG (keep this line here)
  SYSCTL |= SYSJTAGPIN;

  // Initialize state variables
//floatswitch_active = 0;
  floatswitches = 0;
  battery_charge = 0;
  solarpanel_voltage = 0;
  pump_active = 0;
  tryagain_timeelapsed = 0;
  last_sent_warningtext = 0;

  // Read in the saved phone number from memory, if it is there
  memset(phone_number, '\0', MAX_PHONE_LENGTH);
  if(strncmp(PHONE_ADDRESS, "+1", 2) == 0) // Phone numbers start with +1
    strncpy(phone_number, PHONE_ADDRESS, MAX_PHONE_LENGTH); // copy from flash into ram

  // Set up float switches
  FLOAT_PORT_DIR &= ~(FLOATSWITCH_0 | FLOATSWITCH_1 | FLOATSWITCH_2 | FLOATSWITCH_3 | FLOATSWITCH_4);
  FLOAT_PORT_REN |= FLOATSWITCH_0 | FLOATSWITCH_1 | FLOATSWITCH_2 | FLOATSWITCH_3 | FLOATSWITCH_4;
  FLOAT_PORT_OUT |= FLOATSWITCH_0 | FLOATSWITCH_1 | FLOATSWITCH_2 | FLOATSWITCH_3 | FLOATSWITCH_4;

  // Set up water pump and solarpanel on/off
  PUMPSOLAR_PORT_DIR |= PUMP_CONTROL | SOLARPANEL_CONTROL;
  PUMPSOLAR_PORT_OUT &= ~(PUMP_CONTROL | SOLARPANEL_CONTROL);

  // Set up msp430 LEDs
  LED_PORT_DIR |= (LED_MSP | LED_MSP_2);
  LED_PORT_OUT &= ~(LED_MSP | LED_MSP_2);
//  P1DIR |= LED_MSP;
//    P4DIR |= LED_MSP_2;
//  P1OUT &= ~LED_MSP;
//    P4OUT &= ~LED_MSP_2;

  // Set up gsm 'power button' (not used now)
//  P1DIR &= ~POWER_BUTTON;
//  P1REN |= POWER_BUTTON;
//  P1OUT |= POWER_BUTTON;
//  P1IE |= POWER_BUTTON; // interrupts
//  P1IES |= POWER_BUTTON; // high->low transition

  // Set up gsm power status input
  GSM_PORT_DIR &= ~GSM_POWER_STATUS; // input

  // Power on the voltage regulator for the gsm
  GSMPOWER_PORT_DIR |= GSMPOWER_ENABLE_PIN; // output mode
  GSMPOWER_PORT_OUT |= GSMPOWER_ENABLE_PIN;

  // Initialize the uart and ADC, start ADC conversion
  uart_initialize();
  adc_initialize();

  // Wait a bit
  __delay_cycles(1048576); // 1 second

  // Enable watchdog interrupts and interrupts in general
  SFRIE1 |= WDTIE;
  _BIS_SR(GIE);

  // Start conversion
  adc_start_conversion();

  // Check if GSM module is on
  while(!(GSM_PORT_IN & GSM_POWER_STATUS)) // is off
  {
    toggle_gsm_power();
    __delay_cycles(20000000); // wait
  }

  // Send an AT first
	LED_PORT_OUT |= LED_MSP;

	tx_buffer_reset();
  strcpy(tx_buffer, "AT\r\n");
  uart_send_command();

  // Start up Timer A0
  TA0CTL = TACLR; // clear first
  TA0CTL = TASSEL__ACLK | ID__8 | MC__STOP; // auxiliary clock (32.768 kHz), divide by 8 (4096 Hz), interrupt enable, stop mode
  TA0CCTL0 = CCIE; // enable capture/compare interrupt
  TA0CCR0 = 4096; // reduces rate to 1 times/sec
  TA0CTL |= MC__UP; // start the timer in up mode (counts to TA0CCR0 then resets to 0)

  // start the clock
  rtc_initialize();

  // Turn CPU off
  LPM0;

  // Main loop
  while(1)
  {
    // Check if a UART command has finished and respond accordingly
    if(uart_command_has_completed)
    {
      switch(uart_command_state)
      {
      case CommandStateSendingAT:
      {
        if(uart_command_result == UartResultOK)
        {
          // Send ATE0 because we do not need a copy of what we send
            uart_command_state = CommandStateTurnOffEcho;
            tx_buffer_reset();
            strcpy(tx_buffer, "ATE0\r\n");
            uart_send_command();
        }
        break;
      }
      case CommandStateTurnOffEcho: // Got a response after sending AT
      {
        if(uart_command_result == UartResultOK)
        {
          // Send cmgf
          // This puts the cell module into SMS mode, as opposed to data mode
          uart_command_state = CommandStateGoToSMSMode;
          tx_buffer_reset();
          strcpy(tx_buffer, "AT+CMGF=1\r\n");
          uart_send_command();
        }
        break;
      }

      case CommandStateGoToSMSMode: // Got a response after sending CMGF
      {
        if(uart_command_result == UartResultOK)
        {
          LED_PORT_OUT &= ~(LED_MSP | LED_MSP_2); // leds off

          // We are now ready to send a text whenever the system needs to
          uart_enter_idle_mode();
        }
        else
          uart_enter_idle_mode();
        break;
      }

      case CommandStatePrepareWarningSMS: // Got a response after sending CMGS
      {
        if(uart_command_result == UartResultInput)
        {
          // Send the text now
          uart_command_state = CommandStateSendWarningSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "Msg from Sol-Mate: Check your boat; water level is getting high.\r\n\x1A");
          uart_send_command();
        }
        break;
      }

      case CommandStateSendWarningSMS: // Got a response after sending the text
      {
        if(uart_command_result == UartResultOK)
        {
          LED_PORT_OUT &= ~LED_MSP; // red LED off
          sent_text = 1; // Do not send the text again (this is for testing purposes--to send another text you have to restart the MSP)

          // Delete all stored messages.
          uart_command_state = CommandStateDeleteSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "AT+CMGD=1,4\r\n");
          uart_send_command();
        }
        else if(uart_command_result == UartResultError) // sms failed to send
        {
          LED_PORT_OUT |= LED_MSP;

          // Set up timer to try again
          TA2CTL = TACLR;
          TA2CTL = TASSEL__ACLK | ID__8 | MC__STOP;
          TA2CCTL0 = CCIE;
          TA2CCR0 = TIMEOUT_SMS;

          // Prepare again
          uart_command_state = CommandStatePrepareWarningSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "AT+CMGS=\"");
          strcat(tx_buffer, phone_number);
          strcat(tx_buffer, "\"\r\n");

          // Enable timer, go to sleep (uart will be disabled too because LPM2)
          TA2CTL |= MC__UP;
          LPM2;
        }
        else
          uart_enter_idle_mode();

        break;
      }

      case CommandStateUnsolicitedMsg: // Received a message from the cell module
      {
        LED_PORT_OUT |= LED_MSP; // red LED on

        // Check what kind of code this is..
        // --SMS--
        // +CMTI: "SM",3\r\n
        if(strstr(rx_buffer, "+CMTI")) // strstr returns null/0 if not found
        {
          // Find the comma
          char *begin_ptr = strchr(rx_buffer, ',');
          if(!begin_ptr) {
            uart_enter_idle_mode();
            break;
          }
          begin_ptr++; // should point to the beginning of the SMS index we need

          // Find the '\r' which is directly following the last character of the SMS index
          char *end_ptr = strchr(begin_ptr, '\r');
          if(!end_ptr) {
            uart_enter_idle_mode();
            break;
          }

          // Create the command to read the sms
          tx_buffer_reset();
          strcat(tx_buffer, "AT+CMGR=");
          strncat(tx_buffer, begin_ptr, end_ptr - begin_ptr); // SMS index
          strcat(tx_buffer, "\r\n");

          // Send the command
          LED_PORT_OUT &= ~LED_MSP; // red LED on
          uart_command_state = CommandStateReadSMS;
          uart_send_command();
        }
        else // unrecognized
        {
          LED_PORT_OUT &= ~LED_MSP;
          uart_enter_idle_mode();
        }

        break;
      }

      case CommandStateReadSMS:
      {
        LED_PORT_OUT |= LED_MSP; // red LED on
        if(uart_command_result == UartResultOK)
        {
          // +CMGR: "<status>","<origin number>","<??>","<timestamp>"\r\n
          // text contents here\r\n
          // \r\n
          // OK\r\n

          // find the 1st comma
          char *begin_ptr_phone = strchr(rx_buffer, ',');
          if(!begin_ptr_phone || *(begin_ptr_phone+1) != '"') {
            uart_enter_idle_mode();
            break;
          }
          begin_ptr_phone += 2; // Move to the beginning of the number

          // find the ending quotation mark
          char *end_ptr_phone = strchr(begin_ptr_phone, '"');
          if(!end_ptr_phone) {
            uart_enter_idle_mode();
            break;
          }

          // Check if it's too long
          if(end_ptr_phone - begin_ptr_phone > MAX_PHONE_LENGTH) {
            uart_enter_idle_mode();
            break;
          }

          // Look at the contents of the text - it starts right after the first \r\n
          char *begin_ptr_sms = strchr(rx_buffer, '\n');
          if(!begin_ptr_sms) {
            uart_enter_idle_mode();
            break;
          }
          begin_ptr_sms++; // Move to the beginning of the text

          // The text ends right before the next \r\n
          char *end_ptr_sms = strchr(begin_ptr_sms, '\r');
          if(!end_ptr_sms) {
            uart_enter_idle_mode();
            break;
          }

          // Check for the "password"
          if(strstr(begin_ptr_sms, "978SolMate"))
          {
            // copy the phone number into ram
            memset(phone_number, '\0', MAX_PHONE_LENGTH);
            strncpy(phone_number, begin_ptr_phone, end_ptr_phone - begin_ptr_phone);

            // Now copy it into flash memory
            flash_erase(PHONE_ADDRESS);
            flash_write_phone_number(phone_number, MAX_PHONE_LENGTH);

            // Send the user an acknowledgement
            LED_PORT_OUT &= ~LED_MSP; // red LED off
            uart_command_state = CommandStatePreparePhoneSMS;
            tx_buffer_reset();
            strcpy(tx_buffer, "AT+CMGS=\"");
            strncat(tx_buffer, phone_number, MAX_PHONE_LENGTH);
            strcat(tx_buffer, "\"\r\n");
            uart_send_command();
          }
          // Status report?
          else if(strstr(begin_ptr_sms, "What's up"))
          {
            // Send user the status report
            LED_PORT_OUT &= ~LED_MSP;
            uart_command_state = CommandStatePrepareStatusSMS;
            tx_buffer_reset();
            strcpy(tx_buffer, "AT+CMGS=\"");
            strncat(tx_buffer, phone_number, MAX_PHONE_LENGTH);
            strcat(tx_buffer, "\"\r\n");
            uart_send_command();
          }
          else // Unrecognized text
          {
            LED_PORT_OUT &= ~LED_MSP;

            // Delete all stored messages.
            uart_command_state = CommandStateDeleteSMS;
            tx_buffer_reset();
            strcpy(tx_buffer, "AT+CMGD=1,4\r\n");
            uart_send_command();
          }
        }
        else
          uart_enter_idle_mode();

        break;
      }

      case CommandStatePreparePhoneSMS:
      {
        LED_PORT_OUT |= LED_MSP; // red LED on

        if(uart_command_result == UartResultInput)
        {
          // Send the text now
          uart_command_state = CommandStateSendPhoneSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "Msg from Sol-Mate: Your phone number has been successfully changed.\r\n\x1A");
          uart_send_command();
        }
        break;
      }

      case CommandStatePrepareStatusSMS:
      {
        LED_PORT_OUT |= LED_MSP; // red led
        if(uart_command_result == UartResultInput)
        {
          // Put together the status text
          uart_command_state = CommandStateSendStatusSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "Msg from Sol-Mate: Here's your status report.\r\n");

          // Battery status
          if(battery_charge > 228) // 12.9V
            strcat(tx_buffer, "Battery level: Full\r\n");
          else if(battery_charge > 210) // About 50% - 12.55V
            strcat(tx_buffer, "Battery level: Medium\r\n");
          else if(battery_charge > 190) // 12.2V
            strcat(tx_buffer, "Battery level: Low\r\n");
          else
            strcat(tx_buffer, "Battery level: Very Low\r\n");

          // Solar panel charge
          if(solarpanel_voltage > 186)
            strcat(tx_buffer, "Charge rate: High\r\n");
          else if(solarpanel_voltage > 113)
            strcat(tx_buffer, "Charge rate: Medium\r\n");
          else if(solarpanel_voltage > 39)
            strcat(tx_buffer, "Charge rate: Low\r\n");
          else
            strcat(tx_buffer, "Charge rate: None\r\n");

          // Water depth
          int water_level = get_water_level(floatswitches, 5);
          switch(water_level)
          {
            case 0: // No floatswitches are active.
              strcat(tx_buffer, "Water level: None\r\n");
              break;
            case 1: // Lowest floatswitch is active.
              strcat(tx_buffer, "Water level: Very low\r\n");
              break;
            case 2: // Two lowest floatswitches are active.
              strcat(tx_buffer, "Water level: Low\r\n");
              break;
            case 3: // All three floatswitches are active.
              strcat(tx_buffer, "Water level: Medium\r\n");
              break;
            case 4:
              strcat(tx_buffer, "Water level: High\r\n");
              break;
            case 5:
              strcat(tx_buffer, "Water level: Very high\r\n");
              break;
            default: // Any other combination.
              strcat(tx_buffer, "Water level: ERR INVALID READING\r\n");
              break;
          }

          // Bailer
          if(pump_active)
            strcat(tx_buffer, "Water pump: On");
          else
            strcat(tx_buffer, "Water pump: Off");

          strcat(tx_buffer, "\r\n\x1A");
          uart_send_command();
        }
        break;
      }

      case CommandStateSendPhoneSMS:
      {
        if(uart_command_result == UartResultOK)
        {
          // Delete all stored messages.
          uart_command_state = CommandStateDeleteSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "AT+CMGD=1,4\r\n");
          uart_send_command();
        }
        else if(uart_command_result == UartResultError) // sms failed to send
        {
          LED_PORT_OUT |= LED_MSP;

          // Set up timer to try again
          TA2CTL = TACLR;
          TA2CTL = TASSEL__ACLK | ID__8 | MC__STOP;
          TA2CCTL0 = CCIE;
          TA2CCR0 = TIMEOUT_SMS; // 4096 times 10 -> 10 seconds

          // Prepare again
          uart_command_state = CommandStatePreparePhoneSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "AT+CMGS=\"");
          strcat(tx_buffer, phone_number);
          strcat(tx_buffer, "\"\r\n");

          // Enable timer, go to sleep (uart will be disabled too because LPM2)
          TA2CTL |= MC__UP;
          LPM2;
        }

        break;
      }
      case CommandStateSendStatusSMS:
      {
        if(uart_command_result == UartResultOK)
        {
          // Delete all stored messages.
          uart_command_state = CommandStateDeleteSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "AT+CMGD=1,4\r\n");
          uart_send_command();
        }
        else if(uart_command_result == UartResultError) // sms failed to send
        {
          LED_PORT_OUT |= LED_MSP;

          // Set up timer to try again
          TA2CTL = TACLR;
          TA2CTL = TASSEL__ACLK | ID__8 | MC__STOP;
          TA2CCTL0 = CCIE;
          TA2CCR0 = TIMEOUT_SMS; // 4096 times 10 -> 10 seconds

          // Prepare again
          uart_command_state = CommandStatePrepareStatusSMS;
          tx_buffer_reset();
          strcpy(tx_buffer, "AT+CMGS=\"");
          strcat(tx_buffer, phone_number);
          strcat(tx_buffer, "\"\r\n");

          // Enable timer, go to sleep (uart will be disabled too because LPM2)
          TA2CTL |= MC__UP;
          LPM2;
        }

        break;
      }

      case CommandStateDeleteSMS:
      {
        if(uart_command_result == UartResultOK)
          LED_PORT_OUT &= ~LED_MSP; // red LED off

        uart_enter_idle_mode();
        break;
      }
    }

    // Turn CPU off until someone calls LPM0_EXIT (uart interrupt handler will)
    LPM0;
    }
  }
}
Пример #5
0
bool platform_initialize(void* tcb)
{
  OS_ERR osErr;
  
  mainTcb = tcb;
  
  SYS_Init();
  
  GPIO_SetBit(LED_LINK_PORT, LED_LINK_PIN);
  GPIO_Open(LED_LINK_PORT, GPIO_PMD_PMD8_OUTPUT, GPIO_PMD_PMD8_MASK);
  
#if NABTO_ENABLE_LOGGING
  uart_initialize(115200);
#endif
  
  // Initialize OS tick system
  OS_CPU_SysTickInit(SYS_GetHCLKFreq() / OS_CFG_TICK_RATE_HZ);
  
  OSSemCreate(&loggingSemaphore, NULL, 1, &osErr);
  if(osErr != OS_ERR_NONE)
  {
    NABTO_LOG_FATAL(("Unable to create logging semaphore"));
  }
  
  NABTO_LOG_INFO(("Initializing..."));
    
  if (RAK_DriverInit() != RAK_OK)
  {
    NABTO_LOG_FATAL(("Platform initialize failed!"));
  }

  {
    char mac[6];
    if (RAK_GetMacAddr(mac) != RAK_OK)
    {
      NABTO_LOG_FATAL(("RAK_GetMacAddr() failed!"));
    }
    NABTO_LOG_INFO(("MAC: %.2x:%.2x:%.2x:%.2x:%.2x:%.2x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]));
  }
  
  {
    RAK_CONNECT param;
    param.mode = NET_MODE_STA;
    param.sec_mode = PSK_MODE_SEC;
    param.ssid = wifiSsid;
    param.psk = wifiKey;
    param.conn_handle = wifi_callback;
    if (RAK_ConnectAP(&param) != RAK_OK)
    {
      NABTO_LOG_FATAL(("Wifi connect error!"));
    }
  }

  while(linkIsUp == false); // wait for callback to set connection status

  {
    RAK_IPCONFIG dhcp;
    
    if (RAK_IPConfigDHCP(&dhcp) != RAK_OK)
    {
      NABTO_LOG_FATAL(("DHCP error!"));
    }
    
    localAddress = dhcp.addr;
    localMask = dhcp.mask;
    gateway = dhcp.gw;
    dnsServer = dhcp.dnsrv1;
    NABTO_LOG_TRACE(("DHCP: Address=" PRI_IP " mask=" PRI_IP " gateway=" PRI_IP " DNS=" PRI_IP, PRI_IP_FORMAT(localAddress), PRI_IP_FORMAT(localMask), PRI_IP_FORMAT(gateway), PRI_IP_FORMAT(dnsServer)));
  }
  
  memset(sockets, 0, sizeof(sockets));
  
  sendBuffer = RAK_SendMalloc(SEND_BUFFER_SIZE);
  
  return true;
}
Пример #6
0
/** @brief  Vitual COM Port initialization
 *
 *  The function inititalizes the hardware resources of the port used as
 *  the Virtual COM Port.
 *
 *  @return 0 Function failed.
 *  @return 1 Function succeeded.
 */
int32_t USBD_CDC_ACM_PortInitialize(void)
{
    uart_initialize();
    main_cdc_send_event();
    return 1;
}
Пример #7
0
void task_main(int argc, char *argv[])
{
	_is_running = true;

	if (uart_initialize(_device) < 0) {
		PX4_ERR("Failed to initialize UART.");
		return;
	}

	// Subscribe for orb topics
	_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	// Start disarmed
	_armed.armed = false;
	_armed.prearmed = false;

	// Set up poll topic
	px4_pollfd_struct_t fds[1];
	fds[0].fd     = _controls_sub;
	fds[0].events = POLLIN;
	/* Don't limit poll intervall for now, 250 Hz should be fine. */
	//orb_set_interval(_controls_sub, 10);

	// Set up mixer
	if (initialize_mixer(MIXER_FILENAME) < 0) {
		PX4_ERR("Mixer initialization failed.");
		return;
	}

	pwm_limit_init(&_pwm_limit);

	// TODO XXX: this is needed otherwise we crash in the callback context.
	_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc);

	// Main loop
	while (!_task_should_exit) {

		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);

		/* Timed out, do a periodic check for _task_should_exit. */
		if (pret == 0) {
			continue;
		}

		/* This is undesirable but not much we can do. */
		if (pret < 0) {
			PX4_WARN("poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);

			_outputs.timestamp = _controls.timestamp;

			/* do mixing */
			_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);

			/* disable unused ports by setting their output to NaN */
			for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
				_outputs.output[i] = NAN;
			}

			const uint16_t reverse_mask = 0;
			// TODO FIXME: these should probably be params
			const uint16_t disarmed_pwm[4] = {900, 900, 900, 900};
			const uint16_t min_pwm[4] = {1230, 1230, 1230, 1230};
			const uint16_t max_pwm[4] = {1900, 1900, 1900, 1900};
			uint16_t pwm[4];

			// TODO FIXME: pre-armed seems broken
			pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask,
				       disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);


			send_outputs_mavlink(pwm, 4);

			if (_outputs_pub != nullptr) {
				orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);

			} else {
				_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
			}
		}

		bool updated;
		orb_check(_armed_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
		}
	}

	uart_deinitialize();
	orb_unsubscribe(_controls_sub);
	orb_unsubscribe(_armed_sub);

	_is_running = false;

}
Пример #8
0
/*******************************************************************************
* MAIN FUNCTION                                                                *
*******************************************************************************/
int main(void)
{
	unsigned char angle = 0;	// declare a variable to store angle
	unsigned char i = 0,j = 0;
	unsigned char lx = 0, ly = 0, ry = 0;
	unsigned int rx = 0;
	// ensure all the hardware port in zero initially
	PORTA = 0;
	PORTB = 0;
	PORTC = 0;
	PORTD = 0;
	PORTE = 0;

	// Initialize the I/O port direction, this must be configured according to circuit
	// please refer to PTK40A schematic for details
	// TRISX control pin direction, output pin must be configure as '0'
	// while input must be configure as '1'
	TRISA = 0b00010001;
	TRISB = 0b00001111;
	TRISC = 0b10010011;
	TRISD = 0;
	TRISE = 0;

	// Initialize ADC.
	adc_initialize();	//Ensure pin share with analog is being configured to digital correctly

	pwm_initialize();

	uart_initialize();
		
	beep(2); 	//buzzer sound for twice
		
	// PTK40A come SKPS PORT TO USE SKPS
	// SKPS, control DC motor speed and RC servo motor
	// Please connect SKPS at the "Cytron Starter Kit" section
	// Select 9600 as the baud rate on SKPS
	// servo require the PCM at around 20ms period
	// Please move jumper JP9 to SERVO, JP10 to PWM, JP20 & 21 to DC MOTOR
	
	M1 = 1;
	M2 = 0;		// drive motor in a direction
	while(1) 	// create an infinite loop
	{
		lx = uc_skps(p_joy_lx);	//obtain left joystick x axis value
		ly = uc_skps(p_joy_ly); //obtain left joystick y axis value
		rx = uc_skps(p_joy_rx); //obtain right joystick x axis value
		ry = uc_skps(p_joy_ry); //obtain right joystick y axis value
		
		//control dc motor using joyleft
		if(lx==128 && ly==128)
		{
			pwm_set_duty_cycle(0);		
		}	
		//if(lx!=128 && ly!=128)
		else if(ly==0)
		{
			pwm_set_duty_cycle(1000);		//set dc motor speed in differect coordinate
		}
		else if(lx==0)
		{
			pwm_set_duty_cycle(350);
		}
		else if(ly==255)
		{
			pwm_set_duty_cycle(500);
		}
		else if(lx==255)
		{
			pwm_set_duty_cycle(750);
		}
		
		//control servo motor using joyright
		if(ry==0)
		{
		SERVO = 1;		// Servo pin HIGH
		delay_10us(100);
		SERVO = 0;	// Servo pin LOW
		delay_ms(18);	// delay for around 18ms
		}
		else if(rx==0)
		{
		SERVO = 1;		// Servo pin HIGH
		delay_10us(130);
		SERVO = 0;	// Servo pin LOW
		delay_ms(18);	// delay for around 18ms
		}
		else if(ry==255)
		{
		SERVO = 1;		// Servo pin HIGH
		delay_10us(160);
		SERVO = 0;	// Servo pin LOW
		delay_ms(18);	// delay for around 18ms
		}
		else if(rx==255)
		{
		SERVO = 1;		// Servo pin HIGH
		delay_10us(200);
		SERVO = 0;	// Servo pin LOW
		delay_ms(18);	// delay for around 18ms
		}			
	}		
	while(1) continue;	// infinite loop to prevent PIC from reset if there is no more program	
}