C++ PIOS_Assert函数代码示例

static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg)
{
    uint32_t pios_ppm_id;

    PIOS_PPM_Init(&pios_ppm_id, ppm_cfg);

    uint32_t pios_ppm_rcvr_id;
    if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
        PIOS_Assert(0);
    }
    pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
}

/**
 *
 * @brief   Locks a non recursive mutex.
 *
 * @param[in] mtx          pointer to instance of @p struct pios_mutex
 * @param[in] timeout_ms   timeout for acquiring the lock in milliseconds
 *
 * @returns true on success or false on timeout or failure
 *
 */
bool PIOS_Mutex_Lock(struct pios_mutex *mtx, uint32_t timeout_ms)
{
	PIOS_Assert(mtx != NULL);

	portTickType timeout_ticks;
	if (timeout_ms == PIOS_MUTEX_TIMEOUT_MAX)
		timeout_ticks = portMAX_DELAY;
	else
		timeout_ticks = MS2TICKS(timeout_ms);

	return xSemaphoreTake(mtx->mtx_handle, timeout_ticks) == pdTRUE;
}

static void PIOS_Board_configure_hsum(const struct pios_usart_cfg *pios_usart_hsum_cfg,
		const struct pios_com_driver *pios_usart_com_driver,enum pios_hsum_proto *proto,
		ManualControlSettingsChannelGroupsOptions channelgroup)
{
	uintptr_t pios_usart_hsum_id;
	if (PIOS_USART_Init(&pios_usart_hsum_id, pios_usart_hsum_cfg)) {
		PIOS_Assert(0);
	}
	
	uintptr_t pios_hsum_id;
	if (PIOS_HSUM_Init(&pios_hsum_id, pios_usart_com_driver,
			pios_usart_hsum_id, *proto)) {
		PIOS_Assert(0);
	}
	
	uintptr_t pios_hsum_rcvr_id;
	if (PIOS_RCVR_Init(&pios_hsum_rcvr_id, &pios_hsum_rcvr_driver, pios_hsum_id)) {
		PIOS_Assert(0);
	}
	pios_rcvr_group_map[channelgroup] = pios_hsum_rcvr_id;
}

static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void)
{
	struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id;

	bool valid = PIOS_USB_CDC_validate(usb_cdc_dev);
	PIOS_Assert(valid);

	uint32_t DataLength;

	/* Get the number of received data on the selected Endpoint */
	DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep);
	if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) {
		usb_cdc_dev->rx_oversize++;
		DataLength = sizeof(usb_cdc_dev->rx_packet_buffer);
	}

	/* Use the memory interface function to read from the selected endpoint */
	PMAToUserBufferCopy((uint8_t *) usb_cdc_dev->rx_packet_buffer,
			GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep),
			DataLength);

	if (!usb_cdc_dev->rx_in_cb) {
		/* No Rx call back registered, disable the receiver */
		SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK);
		return;
	}

	uint16_t headroom;
	bool need_yield = false;
	uint16_t rc;
	rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context,
				usb_cdc_dev->rx_packet_buffer,
				DataLength,
				&headroom,
				&need_yield);

	if (rc < DataLength) {
		/* Lost bytes on rx */
		usb_cdc_dev->rx_dropped += (DataLength - rc);
	}

	if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) {
		/* We have room for a maximum length message */
		SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID);
	} else {
		/* Not enough room left for a message, apply backpressure */
		SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK);
	}

#if defined(PIOS_INCLUDE_FREERTOS)
	portEND_SWITCHING_ISR(need_yield ? pdTRUE : pdFALSE);
#endif	/* PIOS_INCLUDE_FREERTOS */
}

uint16_t PIOS_COM_MSG_Receive(uintptr_t com_id, uint8_t * msg, uint16_t msg_len)
{
	PIOS_Assert(msg);
	PIOS_Assert(msg_len);

	struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)com_id;

	PIOS_Assert(msg_len == sizeof(com_dev->rx_msg_buffer));

	if (!com_dev->rx_msg_full) {
		/* There's room in our buffer, kick the receiver */
		(com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer));
	} else {
		memcpy(msg, com_dev->rx_msg_buffer, msg_len);
		com_dev->rx_msg_full = false;

		return msg_len;
	}

	return 0;
}

static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg,
		const struct pios_com_driver *pios_usart_com_driver, 
		ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind)
{
	uintptr_t pios_usart_dsm_id;
	if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) {
		PIOS_Assert(0);
	}
	
	uintptr_t pios_dsm_id;
	if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, pios_usart_com_driver,
			pios_usart_dsm_id, *bind)) {
		PIOS_Assert(0);
	}
	
	uintptr_t pios_dsm_rcvr_id;
	if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
		PIOS_Assert(0);
	}
	pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id;
}

void PIOS_USB_RCTX_Update(uint32_t usbrctx_id, const uint16_t channel[], const int16_t channel_min[], const int16_t channel_max[], uint8_t num_channels)
{
    struct pios_usb_rctx_dev *usb_rctx_dev = (struct pios_usb_rctx_dev *)usbrctx_id;

    bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev);

    PIOS_Assert(valid);

    if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) {
        return;
    }

    for (uint8_t i = 0;
         i < PIOS_USB_RCTX_NUM_CHANNELS && i < num_channels;
         i++) {
        int16_t min  = channel_min[i];
        int16_t max  = channel_max[i];
        uint16_t val = channel[i];

        if (channel_min[i] > channel_max[i]) {
            /* This channel is reversed, flip min and max */
            min = channel_max[i];
            max = channel_min[i];

            /* and flip val to be an offset from the lower end of the range */
            val = channel_min[i] - channel[i] + channel_max[i];
        }

        /* Scale channel linearly between min and max */
        if (min == max) {
            val = 0;
        } else {
            if (val < min) {
                val = min;
            }
            if (val > max) {
                val = max;
            }

            val = (val - min) * (65535 / (max - min));
        }

        usb_rctx_dev->report.vals[i] = val;
    }

    if (GetEPTxStatus(usb_rctx_dev->cfg->data_tx_ep) == EP_TX_VALID) {
        /* Endpoint is already transmitting */
        return;
    }

    PIOS_USB_RCTX_SendReport(usb_rctx_dev);
}

/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBuffer(uintptr_t com_id, const uint8_t *buffer, uint16_t len)
{
	struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id;

	if (!PIOS_COM_validate(com_dev)) {
		/* Undefined COM port for this board (see pios_board.c) */
		return -1;
	}

	PIOS_Assert(com_dev->has_tx);

	uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx);
	uint32_t bytes_to_send = len;
	while (bytes_to_send) {
		uint32_t frag_size;

		if (bytes_to_send > max_frag_len) {
			frag_size = max_frag_len;
		} else {
			frag_size = bytes_to_send;
		}
		int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size);
		if (rc >= 0) {
			bytes_to_send -= rc;
			buffer += rc;
		} else {
			switch (rc) {
			case -1:
				/* Device is invalid, this will never work */
				return -1;
			case -2:
				/* Device is busy, wait for the underlying device to free some space and retry */
				/* Make sure the transmitter is running while we wait */
				if (com_dev->driver->tx_start) {
					(com_dev->driver->tx_start)(com_dev->lower_id,
								fifoBuf_getUsed(&com_dev->tx));
				}
#if defined(PIOS_INCLUDE_FREERTOS)
				if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) {
					return -3;
				}
#endif
				continue;
			default:
				/* Unhandled return code */
				return rc;
			}
		}
	}

	return len;
}

static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg)
{
    /* Set up the receiver port.  Later this should be optional */
    uint32_t pios_pwm_id;

    PIOS_PWM_Init(&pios_pwm_id, pwm_cfg);

    uint32_t pios_pwm_rcvr_id;
    if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
        PIOS_Assert(0);
    }
    pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
}

uint8_t PIOS_RCVR_GetRSSI(uint32_t rcvr_id)
{
  struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;

  if (!PIOS_RCVR_validate(rcvr_dev)) {
    /* Undefined RCVR port for this board (see pios_board.c) */
    PIOS_Assert(0);
  }

  PIOS_DEBUG_Assert(rcvr_dev->driver->rssi);

  return rcvr_dev->driver->rssi(rcvr_dev->lower_id);
}

int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
{
  struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;

  if (!PIOS_RCVR_validate(rcvr_dev)) {
    /* Undefined RCVR port for this board (see pios_board.c) */
    PIOS_Assert(0);
  }

  PIOS_DEBUG_Assert(rcvr_dev->driver->read);

  return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
}

/**
 * Find the first sector for a file
 * @param[in] streamfs the file system handle
 * @return the sector for the new file
 *
 * @NOTE: Must be called while holding the flash transaction lock
 */
static int32_t streamfs_find_new_sector(struct streamfs_state *streamfs)
{
    // No files on file system
    if (streamfs->max_file_id < 0) {
        return 0;
    }

    int32_t last_sector = streamfs_find_last_arena(streamfs, streamfs->max_file_id);
    PIOS_Assert(last_sector >= 0);

    uint16_t num_arenas = streamfs->partition_size / streamfs->cfg->arena_size;
    return (last_sector + 1) % num_arenas;
}

void PIOS_Board_Init()
{
	/* Delay system */
	PIOS_DELAY_Init();

#if defined(PIOS_INCLUDE_LED)
	PIOS_LED_Init(&pios_led_cfg);
#endif	/* PIOS_INCLUDE_LED */

#if defined(PIOS_INCLUDE_FLASH)
	/* Inititialize all flash drivers */
	PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg);

	/* Register the partition table */
	PIOS_FLASH_register_partition_table(pios_flash_partition_table, NELEMENTS(pios_flash_partition_table));
#endif	/* PIOS_INCLUDE_FLASH */

#if defined(PIOS_INCLUDE_USB)
	/* Initialize board specific USB data */
	PIOS_USB_BOARD_DATA_Init();

	/* Activate the HID-only USB configuration */
	PIOS_USB_DESC_HID_ONLY_Init();

	uintptr_t pios_usb_id;
	PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);

#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
	uintptr_t pios_usb_hid_id;
	if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
		PIOS_Assert(0);
	}
	if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
		PIOS_Assert(0);
	}
#endif	/* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */

#endif	/* PIOS_INCLUDE_USB */
}

int32_t PIOS_USB_Init(uintptr_t * usb_id, const struct pios_usb_cfg * cfg)
{
	PIOS_Assert(usb_id);
	PIOS_Assert(cfg);

	struct pios_usb_dev * usb_dev;

	usb_dev = (struct pios_usb_dev *) PIOS_USB_alloc();
	if (!usb_dev) goto out_fail;

	/* Bind the configuration to the device instance */
	usb_dev->cfg = cfg;

	PIOS_USB_Reenumerate();

	/*
	 * This is a horrible hack to make this available to
	 * the interrupt callbacks.  This should go away ASAP.
	 */
	pios_usb_com_id = (uintptr_t) usb_dev;

	/* Enable the USB Interrupts */
	NVIC_Init((NVIC_InitTypeDef*)&usb_dev->cfg->irq.init);

	/* Select USBCLK source */
	RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5);
	/* Enable the USB clock */
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE);

	USB_Init();
	USB_SIL_Init();

	*usb_id = (uintptr_t) usb_dev;

	return 0;		/* No error */

out_fail:
	return(-1);
}

/**
 * @brief Callback used to indicate a transmission from device INto host completed
 * Checks if any data remains, pads it into HID packet and sends.
 */
static void PIOS_USB_HID_EP_IN_Callback(void)
{
	struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id;

	bool valid = PIOS_USB_HID_validate(usb_hid_dev);
	PIOS_Assert(valid);

	if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) {
		return;
	}

	PIOS_USB_HID_SendReport(usb_hid_dev);
}