Пример #1
0
/**
 * Initialise the module.  Called before the start function
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t SensorsInitialize(void)
{
    GyrosInitialize();
    GyrosBiasInitialize();
    AccelsInitialize();
    MagnetometerInitialize();
    MagBiasInitialize();
    RevoCalibrationInitialize();
    AttitudeSettingsInitialize();

    rotate = 0;

    RevoCalibrationConnectCallback(&settingsUpdatedCb);
    AttitudeSettingsConnectCallback(&settingsUpdatedCb);

    return 0;
}
Пример #2
0
/**
 * Initialise the module.  Called before the start function
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t SensorsInitialize(void)
{

	accel_bias[0] = rand_gauss() / 10;
	accel_bias[1] = rand_gauss() / 10;
	accel_bias[2] = rand_gauss() / 10;

	AccelsInitialize();
	AttitudeSimulatedInitialize();
	BaroAltitudeInitialize();
	BaroAirspeedInitialize();
	GyrosInitialize();
	GyrosBiasInitialize();
	GPSPositionInitialize();
	GPSVelocityInitialize();
	MagnetometerInitialize();
	MagBiasInitialize();

	return 0;
}
Пример #3
0
/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t AttitudeInitialize(void)
{
    AttitudeActualInitialize();
    SensorSettingsInitialize();
    AttitudeSettingsInitialize();
    AccelsInitialize();
    GyrosInitialize();

    // Initialize quaternion
    AttitudeActualData attitude;
    AttitudeActualGet(&attitude);
    attitude.q1 = 1;
    attitude.q2 = 0;
    attitude.q3 = 0;
    attitude.q4 = 0;
    AttitudeActualSet(&attitude);

    // Cannot trust the values to init right above if BL runs
    gyro_correct_int[0] = 0;
    gyro_correct_int[1] = 0;
    gyro_correct_int[2] = 0;

    q[0] = 1;
    q[1] = 0;
    q[2] = 0;
    q[3] = 0;
    for(uint8_t i = 0; i < 3; i++)
        for(uint8_t j = 0; j < 3; j++)
            Rsb[i][j] = 0;

    trim_requested = false;

    AttitudeSettingsConnectCallback(&settingsUpdatedCb);
    SensorSettingsConnectCallback(&settingsUpdatedCb);

    return 0;
}
Пример #4
0
/**
 * PIOS_Board_Init()
 * initializes all the core systems on this specific hardware
 * called from System/openpilot.c
 */
void PIOS_Board_Init(void) {

	/* Delay system */
	PIOS_DELAY_Init();

	/* Initialize UAVObject libraries */
	EventDispatcherInitialize();
	UAVObjInitialize();
	UAVObjectsInitializeAll();

	AccelsInitialize();
	BaroAltitudeInitialize();
	MagnetometerInitialize();
	GPSPositionInitialize();
	GyrosInitialize();
	GyrosBiasInitialize();

	/* Initialize the alarms library */
	AlarmsInitialize();

	/* Initialize the task monitor library */
	TaskMonitorInitialize();

#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1
	{
		uint32_t pios_tcp_telem_rf_id;
		if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) {
			PIOS_Assert(0);
		}

		uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
		uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
		PIOS_Assert(rx_buffer);
		PIOS_Assert(tx_buffer);
		if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id,
						  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
						  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
			PIOS_Assert(0);
		}
	}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */

#if defined(PIOS_INCLUDE_TELEMETRY_RF) && 0
	{
		uint32_t pios_udp_telem_rf_id;
		if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
			PIOS_Assert(0);
		}
		
		uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
		uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
		PIOS_Assert(rx_buffer);
		PIOS_Assert(tx_buffer);
		if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
						  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
						  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
			PIOS_Assert(0);
		}
	}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */


#if defined(PIOS_INCLUDE_GPS)
	{
		uint32_t pios_tcp_gps_id;
		if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) {
			PIOS_Assert(0);
		}
		uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
		PIOS_Assert(rx_buffer);
		if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id,
				  rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
				  NULL, 0)) {
			PIOS_Assert(0);
		}
	}
#endif	/* PIOS_INCLUDE_GPS */
#endif

#if defined(PIOS_INCLUDE_GCSRCVR)
	GCSReceiverInitialize();
	uint32_t pios_gcsrcvr_id;
	PIOS_GCSRCVR_Init(&pios_gcsrcvr_id);
	uint32_t pios_gcsrcvr_rcvr_id;
	if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) {
		PIOS_Assert(0);
	}
	pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;
#endif	/* PIOS_INCLUDE_GCSRCVR */

}