立创梁山派开发板-21年电赛F题-送药小车-国产IMU移植及姿态解算

#include <stdio.h>

#include <rtthread.h>

#include <board.h>

#include <gd32f4xx_libopt.h>

#include <qmi8658x_port.h>



void spi_gpio_config(void)

{

    // SPI CS gpio

    rcu_periph_clock_enable(SPI_CS_GPIO_CLK);

    gpio_mode_set(SPI_CS_GPIO_PORT, GPIO_MODE_OUTPUT, SPI_CS_GPIO_PUPD,

                  SPI_CS_GPIO_PIN);

    gpio_output_options_set(SPI_CS_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ,

                            SPI_CS_GPIO_PIN);



    // SPI SCK gpio init

    rcu_periph_clock_enable(SPI_SCK_GPIO_CLK);

    gpio_mode_set(SPI_SCK_GPIO_PORT, GPIO_MODE_AF, SPI_SCK_GPIO_PUPD,

                  SPI_SCK_GPIO_PIN);

    gpio_af_set(SPI_SCK_GPIO_PORT, SPI_SCK_GPIO_ALT_FUNC, SPI_SCK_GPIO_PIN);



    // SPI MISO gpio init

    rcu_periph_clock_enable(SPI_MISO_GPIO_CLK);

    gpio_mode_set(SPI_MISO_GPIO_PORT, GPIO_MODE_AF, SPI_MISO_GPIO_PUPD,

                  SPI_MISO_GPIO_PIN);

    gpio_af_set(SPI_MISO_GPIO_PORT, SPI_MISO_GPIO_ALT_FUNC, SPI_MISO_GPIO_PIN);



    // SPI MOSI gpio init

    rcu_periph_clock_enable(SPI_MOSI_GPIO_CLK);

    gpio_mode_set(SPI_MOSI_GPIO_PORT, GPIO_MODE_AF, SPI_MOSI_GPIO_PUPD,

                  SPI_MOSI_GPIO_PIN);

    gpio_af_set(SPI_MOSI_GPIO_PORT, SPI_MOSI_GPIO_ALT_FUNC, SPI_MOSI_GPIO_PIN);



    SET_SPI_NSS_HIGH();

}

void spi_config(void)

{

    spi_parameter_struct spi_init_struct;



    rcu_periph_clock_enable(RCU_SPI3);

    /* configure SPI3 parameter */

    spi_init_struct.trans_mode = SPI_TRANSMODE_FULLDUPLEX;

    spi_init_struct.device_mode = SPI_MASTER;

    spi_init_struct.frame_size = SPI_FRAMESIZE_8BIT;

    spi_init_struct.clock_polarity_phase = SPI_CK_PL_LOW_PH_1EDGE;

    spi_init_struct.nss = SPI_NSS_SOFT;

    spi_init_struct.prescale = SPI_PSC_16;

    spi_init_struct.endian = SPI_ENDIAN_MSB;

    spi_init(SPI3, &spi_init_struct);

}



uint8_t spi_send_byte(uint8_t byte)

{

    /* loop while data register in not emplty */

    while (RESET == spi_i2s_flag_get(SPI3, SPI_FLAG_TBE))

        ;



    /* send byte through the SPI3 peripheral */

    spi_i2s_data_transmit(SPI3, byte);



    /* wait to receive a byte */

    while (RESET == spi_i2s_flag_get(SPI3, SPI_FLAG_RBNE))

        ;



    /* return the byte read from the SPI bus */

    return (spi_i2s_data_receive(SPI3));

}

uint8_t spi_flash_read_byte(void) { return (spi_send_byte(0xff)); }



uint8_t qst_imu_spi_write(uint8_t reg, uint8_t value)

{

    spi_send_byte(reg & 0x7f);

    spi_send_byte(value);

    return 0;

}



uint8_t qst_imu_spi_write_bytes(uint8_t reg, uint8_t *value, uint8_t len)

{

    uint8_t *p = value;

    static uint32_t i = 0;

    for (i = 0; i < len; i++)

    {

        qst_imu_spi_write(reg, *p++);

    }

    return 0;

}



uint8_t qst_imu_spi_read(uint32_t reg, uint8_t value)

{

    spi_send_byte(reg | 0x80);

    return spi_flash_read_byte();

}

uint8_t qst_imu_spi_read_bytes(uint8_t reg, void *buffer, uint8_t len)

{

    static uint32_t i = 0;

    uint8_t *p = buffer;

    spi_send_byte(reg | 0x80);

    for (i = 0; i < len; i++)

    {

        *p++ = spi_flash_read_byte();

    }

    return 0;

}



int sensor_spi_init(void)

{

    spi_gpio_config();

    spi_config();

    spi_enable(SPI3);

    return RT_EOK;

}



INIT_BOARD_EXPORT(sensor_spi_init);

<div data-page-id="LD0FdBNHfo5JSaxj4pRcVl0WnCd" data-docx-has-block-data="false"><pre class="ace-line ace-line old-record-id-NPFPdFg1BoNCjkx1tlNc0hXpnLf"><code class="language-C" data-wrap="false">#include "../../Fusion/Fusion.h"

#include <stdbool.h>

#include <stdio.h>



#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period



int main() {

    FusionAhrs ahrs;

    FusionAhrsInitialise(&ahrs);



    while (true) { // this loop should repeat each time new gyroscope data is available

        const FusionVector gyroscope = {0.0f, 0.0f, 0.0f}; // replace this with actual gyroscope data in degrees/s

        const FusionVector accelerometer = {0.0f, 0.0f, 1.0f}; // replace this with actual accelerometer data in g



        FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD);



        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));



        printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);

    }

}</code></pre></div><span data-lark-record-data="{&quot;rootId&quot;:&quot;LD0FdBNHfo5JSaxj4pRcVl0WnCd&quot;,&quot;text&quot;:{&quot;initialAttributedTexts&quot;:{&quot;text&quot;:{&quot;0&quot;:&quot;#include \&quot;../../Fusion/Fusion.h\&quot;\n#include <stdbool.h>\n#include <stdio.h>\n\n#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period\n\nint main() {\n    FusionAhrs ahrs;\n    FusionAhrsInitialise(&ahrs);\n\n    while (true) { // this loop should repeat each time new gyroscope data is available\n        const FusionVector gyroscope = {0.0f, 0.0f, 0.0f}; // replace this with actual gyroscope data in degrees/s\n        const FusionVector accelerometer = {0.0f, 0.0f, 1.0f}; // replace this with actual accelerometer data in g\n\n        FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD);\n\n        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));\n\n        printf(\&quot;Roll %0.1f, Pitch %0.1f, Yaw %0.1f\\n\&quot;, euler.angle.roll, euler.angle.pitch, euler.angle.yaw);\n    }\n}&quot;},&quot;attribs&quot;:{&quot;0&quot;:&quot;*0|k+n3*0+1&quot;}},&quot;apool&quot;:{&quot;numToAttrib&quot;:{&quot;0&quot;:[&quot;author&quot;,&quot;7209868880213491740&quot;]},&quot;nextNum&quot;:1}},&quot;type&quot;:&quot;text&quot;,&quot;referenceRecordMap&quot;:{},&quot;extra&quot;:{&quot;mention_page_title&quot;:{},&quot;external_mention_url&quot;:{}},&quot;isKeepQuoteContainer&quot;:false,&quot;isFromCode&quot;:true,&quot;selection&quot;:[{&quot;id&quot;:100,&quot;type&quot;:&quot;text&quot;,&quot;selection&quot;:{&quot;start&quot;:0,&quot;end&quot;:832},&quot;recordId&quot;:&quot;NPFPdFg1BoNCjkx1tlNc0hXpnLf&quot;}],&quot;payloadMap&quot;:{},&quot;isCut&quot;:false}" data-lark-record-format="docx/text" class="lark-record-clipboard"></span>

/**

 -  [url=home.php?mod=space&uid=247401]@brief[/url]  姿态解算软件定时器回调函数

 -  [url=home.php?mod=space&uid=536309]@NOTE[/url]   None

 -  @param  parameter

 -  @retval None

   */

static void att_thread_timer_callback(void *parameter)

{

    qmi8658_read_xyz(acc, gyro);

    accelerometer.array[0] = (float)((acc[0]) / 9.807f);

    accelerometer.array[1] = (float)((acc[1]) / 9.807f);

    accelerometer.array[2] = (float)((acc[2]) / 9.807f);

    gyroscope.array[0] =

        (float)((gyro[0]) * (180.0f / 3.14159265358979323846f));

    gyroscope.array[1] =

        (float)((gyro[1]) * (180.0f / 3.14159265358979323846f));

    gyroscope.array[2] =

        (float)((gyro[2]) * (180.0f / 3.14159265358979323846f));



    FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer,

                                   SAMPLE_PERIOD);

    const FusionEuler euler =

        FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));

    fusion_data.euler.euler.pitch = euler.angle.pitch;

    fusion_data.euler.euler.roll = euler.angle.roll;

    fusion_data.euler.euler.yaw = euler.angle.yaw;

    fusion_data.quaternion.quaternion.q0 = ahrs.quaternion.element.w;

    fusion_data.quaternion.quaternion.q1 = ahrs.quaternion.element.x;

    fusion_data.quaternion.quaternion.q2 = ahrs.quaternion.element.y;

    fusion_data.quaternion.quaternion.q3 = ahrs.quaternion.element.z;

    mcn_publish(MCN_HUB(fusion_topic), &fusion_data);

}