altos: Make cmd number parsing functions return value

[fw/altos] / src / drivers / ao_mpu9250.c

diff --git a/src/drivers/ao_mpu9250.c b/src/drivers/ao_mpu9250.c
index ae8dacd0a2eee8544e6de5e8061c316ffaacf17b..9dce9f1a5a0d4f79f241c50e2a238a09d2b2e906 100644 (file)
--- a/src/drivers/ao_mpu9250.c
+++ b/src/drivers/ao_mpu9250.c
@@ -472,10 +472,9 @@ ao_mpu9250_read(void)
        uint8_t addr;
        uint8_t val;
 
        uint8_t addr;
        uint8_t val;
 

-       ao_cmd_hex();
+       addr = ao_cmd_hex();

        if (ao_cmd_status != ao_cmd_success)
                return;
        if (ao_cmd_status != ao_cmd_success)
                return;

-       addr = ao_cmd_lex_i;

        ao_mpu9250_spi_get();
        val = _ao_mpu9250_reg_read(addr);
        ao_mpu9250_spi_put();
        ao_mpu9250_spi_get();
        val = _ao_mpu9250_reg_read(addr);
        ao_mpu9250_spi_put();


@@ -488,14 +487,12 @@ ao_mpu9250_write(void)
        uint8_t addr;
        uint8_t val;
 
        uint8_t addr;
        uint8_t val;
 

-       ao_cmd_hex();
+       addr = ao_cmd_hex();

        if (ao_cmd_status != ao_cmd_success)
                return;
        if (ao_cmd_status != ao_cmd_success)
                return;

-       addr = ao_cmd_lex_i;
-       ao_cmd_hex();
+       val = ao_cmd_hex();

        if (ao_cmd_status != ao_cmd_success)
                return;
        if (ao_cmd_status != ao_cmd_success)
                return;

-       val = ao_cmd_lex_i;

        printf("Addr %02x val %02x\n", addr, val);
        ao_mpu9250_spi_get();
        _ao_mpu9250_reg_write(addr, val);
        printf("Addr %02x val %02x\n", addr, val);
        ao_mpu9250_spi_get();
        _ao_mpu9250_reg_write(addr, val);


@@ -508,10 +505,9 @@ ao_mpu9250_mag_read(void)
        uint8_t addr;
        uint8_t val;
 
        uint8_t addr;
        uint8_t val;
 

-       ao_cmd_hex();
+       addr = ao_cmd_hex();

        if (ao_cmd_status != ao_cmd_success)
                return;
        if (ao_cmd_status != ao_cmd_success)
                return;

-       addr = ao_cmd_lex_i;

        ao_mpu9250_spi_get();
        val = _ao_mpu9250_mag_reg_read(addr);
        ao_mpu9250_spi_put();
        ao_mpu9250_spi_get();
        val = _ao_mpu9250_mag_reg_read(addr);
        ao_mpu9250_spi_put();


@@ -524,14 +520,12 @@ ao_mpu9250_mag_write(void)
        uint8_t addr;
        uint8_t val;
 
        uint8_t addr;
        uint8_t val;
 

-       ao_cmd_hex();
+       addr = ao_cmd_hex();

        if (ao_cmd_status != ao_cmd_success)
                return;
        if (ao_cmd_status != ao_cmd_success)
                return;

-       addr = ao_cmd_lex_i;
-       ao_cmd_hex();
+       val = ao_cmd_hex();

        if (ao_cmd_status != ao_cmd_success)
                return;
        if (ao_cmd_status != ao_cmd_success)
                return;

-       val = ao_cmd_lex_i;

        printf("Addr %02x val %02x\n", addr, val);
        ao_mpu9250_spi_get();
        _ao_mpu9250_mag_reg_write(addr, val);
        printf("Addr %02x val %02x\n", addr, val);
        ao_mpu9250_spi_get();
        _ao_mpu9250_mag_reg_write(addr, val);