Ejemplo n.º 1
0
/**
 * Module thread, should not return.
 */
static void AttitudeTask(void *parameters)
{
	uint8_t init = 0;
	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

	PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);

	// Keep flash CS pin high while talking accel
	PIOS_FLASH_DISABLE;
	PIOS_ADXL345_Init();


	// Force settings update to make sure rotation loaded
	settingsUpdatedCb(AttitudeSettingsHandle());

	// Main task loop
	while (1) {

		FlightStatusData flightStatus;
		FlightStatusGet(&flightStatus);

		if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
			// For first 7 seconds use accels to get gyro bias
			accelKp = 1;
			accelKi = 0.9;
			yawBiasRate = 0.23;
			init = 0;
		}
		else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
			accelKp = 1;
			accelKi = 0.9;
			yawBiasRate = 0.23;
			init = 0;
		} else if (init == 0) {
			// Reload settings (all the rates)
			AttitudeSettingsAccelKiGet(&accelKi);
			AttitudeSettingsAccelKpGet(&accelKp);
			AttitudeSettingsYawBiasRateGet(&yawBiasRate);
			init = 1;
		}

		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);

		AttitudeRawData attitudeRaw;
		AttitudeRawGet(&attitudeRaw);
		updateSensors(&attitudeRaw);
		updateAttitude(&attitudeRaw);
		AttitudeRawSet(&attitudeRaw);

	}
}
Ejemplo n.º 2
0
/**
 * Module thread, should not return.
 */
static void AttitudeTask(void *parameters)
{
    AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

    // Force settings update to make sure rotation loaded
    settingsUpdatedCb(AttitudeSettingsHandle());

    enum complimentary_filter_status complimentary_filter_status;
    complimentary_filter_status = CF_POWERON;

    uint32_t arming_count = 0;

    // Main task loop
    while (1) {

        FlightStatusData flightStatus;
        FlightStatusGet(&flightStatus);

        if (complimentary_filter_status == CF_POWERON) {

            complimentary_filter_status = (xTaskGetTickCount() > 1000) ?
                                          CF_INITIALIZING : CF_POWERON;

        } else if(complimentary_filter_status == CF_INITIALIZING &&
                  (xTaskGetTickCount() < 7000) &&
                  (xTaskGetTickCount() > 1000)) {

            // For first 7 seconds use accels to get gyro bias
            accelKp = 0.1f + 0.1f * (xTaskGetTickCount() < 4000);
            accelKi = 0.1;
            yawBiasRate = 0.1;
            accel_filter_enabled = false;

        } else if (zero_during_arming &&
                   (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {

            // Use a rapidly decrease accelKp to force the attitude to snap back
            // to level and then converge more smoothly
            if (arming_count < 20)
                accelKp = 1.0f;
            else if (accelKp > 0.1f)
                accelKp -= 0.01f;
            arming_count++;

            accelKi = 0.1f;
            yawBiasRate = 0.1f;
            accel_filter_enabled = false;

            // Indicate arming so that after arming it reloads
            // the normal settings
            if (complimentary_filter_status != CF_ARMING) {
                accumulate_gyro_zero();
                complimentary_filter_status = CF_ARMING;
                accumulating_gyro = true;
            }

        } else if (complimentary_filter_status == CF_ARMING ||
                   complimentary_filter_status == CF_INITIALIZING) {

            // Reload settings (all the rates)
            AttitudeSettingsAccelKiGet(&accelKi);
            AttitudeSettingsAccelKpGet(&accelKp);
            AttitudeSettingsYawBiasRateGet(&yawBiasRate);
            if(accel_alpha > 0.0f)
                accel_filter_enabled = true;

            // If arming that means we were accumulating gyro
            // samples.  Compute new bias.
            if (complimentary_filter_status == CF_ARMING) {
                accumulate_gyro_compute();
                accumulating_gyro = false;
                arming_count = 0;
            }

            // Indicate normal mode to prevent rerunning this code
            complimentary_filter_status = CF_NORMAL;
        }

        PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);

        AccelsData accels;
        GyrosData gyros;
        int32_t retval = 0;

        retval = updateSensorsCC3D(&accels, &gyros);

        // During power on set to angle from accel
        if (complimentary_filter_status == CF_POWERON) {
            float RPY[3];
            float theta = atan2f(accels.x, -accels.z);
            RPY[1] = theta * RAD2DEG;
            RPY[0] = atan2f(-accels.y, -accels.z / cosf(theta)) * RAD2DEG;
            RPY[2] = 0;
            RPY2Quaternion(RPY, q);
        }

        // Only update attitude when sensor data is good
        if (retval != 0)
            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
        else {
            // Do not update attitude data in simulation mode
            if (!AttitudeActualReadOnly())
                updateAttitude(&accels, &gyros);

            AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
        }
    }
}