C++ (Cpp) imu_adjust_alignment Examples

Example #1

File: imu_booz_hw.c Project: robotang/wasp

void 
imu_init(void)
{
    /* initialises neutrals */
    RATES_ASSIGN(booz_imu.gyro_neutral,  IMU_NEUTRAL_GYRO_P,  IMU_NEUTRAL_GYRO_Q,  IMU_NEUTRAL_GYRO_R);
    VECT3_ASSIGN(booz_imu.accel_neutral, IMU_NEUTRAL_ACCEL_X, IMU_NEUTRAL_ACCEL_Y, IMU_NEUTRAL_ACCEL_Z);
    VECT3_ASSIGN(booz_imu.mag_neutral,   IMU_NEUTRAL_MAG_X,   IMU_NEUTRAL_MAG_Y,   IMU_NEUTRAL_MAG_Z);

    /* initialise IMU alignment */
    imu_adjust_alignment(IMU_ALIGNMENT_BODY_TO_IMU_PHI, IMU_ALIGNMENT_BODY_TO_IMU_THETA, IMU_ALIGNMENT_BODY_TO_IMU_PSI);

    imu_spi_selected = SPI_NONE;
    do_max1168_read = FALSE;

    /* setup pins for SSP (SCK, MISO, MOSI) */
    PINSEL1 |= SSP_PINSEL1_SCK  | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;

    /* setup SSP */
    SSPCR0 = SSPCR0_VAL;;
    SSPCR1 = SSPCR1_VAL;
    SSPCPSR = 0x02;

    /* initialize interrupt vector */
    VICIntSelect &= ~VIC_BIT( VIC_SPI1 );  /* SPI1 selected as IRQ */
    VICIntEnable = VIC_BIT( VIC_SPI1 );    /* enable it            */
    _VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
    _VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR;      /* address of the ISR   */

    max1168_init();
    micromag_init();
}

Example #2

File: settings.c Project: OpenUAS/wasp

static inline bool_t
set_setting_float(uint8_t id, float val)
{
    bool_t ret = TRUE;

    switch (id)
    {
        case SETTING_ID_IMU_ALIGNMENT_BODY_TO_IMU_PHI:
            imu_adjust_alignment(val, booz_imu.body_to_imu_theta, booz_imu.body_to_imu_psi);
            break;
        case SETTING_ID_IMU_ALIGNMENT_BODY_TO_IMU_THETA:
            imu_adjust_alignment(booz_imu.body_to_imu_phi, val, booz_imu.body_to_imu_psi);
            break;
        case SETTING_ID_IMU_ALIGNMENT_BODY_TO_IMU_PSI:
            imu_adjust_alignment(booz_imu.body_to_imu_phi, booz_imu.body_to_imu_theta, val);
            break;
        default:
            ret = FALSE;
    }

    return ret;
}