MP LedTest code explanation.

[Leonardo Garberoglio]

Hi team, I'm trying to port MP LedTest to Nucleo board project (STM32).
Rigth now we have freeRTOS runing and a couple of task, like MPU data reading.
I have some problem to understand what this code mean:

void udb_heartbeat_callback(void)
{
    if (calib_countdown) {
        udb_pwOut[ROLL_OUTPUT_CHANNEL]    = 3000;
        udb_pwOut[PITCH_OUTPUT_CHANNEL]   = 3000;
        udb_pwOut[YAW_OUTPUT_CHANNEL]     = 3000;
        udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = 3000;
        udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = 3000;
        udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = 3000;
    } else if (eepromSuccess == 0 && eepromFailureFlashCount) {
        // eeprom failure!
        DPRINT("eeprom failure!\r\n");
        if (udb_heartbeat_counter % 6 == 0) {
            udb_led_toggle(LED_RED);
            udb_led_toggle(LED_GREEN);
            udb_led_toggle(LED_BLUE);
            udb_led_toggle(LED_ORANGE);
            eepromFailureFlashCount--;
        }
    } else {
        union longww accum;

        accum.WW = __builtin_mulss(y_rate, 4000);
        udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

        accum.WW = __builtin_mulss(x_rate, 4000);
        udb_pwOut[PITCH_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

        accum.WW = __builtin_mulss(z_rate, 4000);
        udb_pwOut[YAW_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

        accum.WW = __builtin_mulss(x_accel, 4000);
        udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

        accum.WW = __builtin_mulss(y_accel, 4000);
        udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

        accum.WW = __builtin_mulss(z_accel, 4000);
        udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

        if ((udb_heartbeat_counter / 600) % 2 == 0) {
            LED_RED = LED_ON;
            LED_ORANGE = ((abs(udb_pwOut[ROLL_OUTPUT_CHANNEL]  - 3000) > RATE_THRESHOLD_LED) ? LED_ON : LED_OFF);
            LED_BLUE   = ((abs(udb_pwOut[PITCH_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? LED_ON : LED_OFF);
            LED_GREEN  = ((abs(udb_pwOut[YAW_OUTPUT_CHANNEL]   - 3000) > RATE_THRESHOLD_LED) ? LED_ON : LED_OFF);
        } else {
            LED_RED = LED_OFF;
            LED_ORANGE = ((abs(udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? LED_ON : LED_OFF);
            LED_BLUE   = ((abs(udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? LED_ON : LED_OFF);
            LED_GREEN  = ((abs(udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? LED_ON : LED_OFF);
        }
    }
}

The first part initialize PWM output to 3000 while the first 10 step of calib_countdown.
But I don't know what this code mean:
        accum.WW = __builtin_mulss(y_rate, 4000);
        udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum take the value of y_rate times 4000. Why 4000? what is the purpose of this operation?
then the result is add to 3000 and filtered with udb_servo_pulsesat. what is the y_rate min and max value? I don't finde where y_rate is divide by gyro gain, I mean where raw data is scaled to obtain DPS value...

Then there is an if that toggle between accel and gyro. This part of the code I understand. If the value stored as a PWM output is greater than a threshold (200 / 120) the LED turns on, else the led turns off.

Thank, Leo

[Peter Hollands]

Leonardo,

Your wrote "But I don't know what this code mean:

        accum.WW = __builtin_mulss(y_rate, 4000);
        udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum take the value of y_rate times 4000. Why 4000? what is the purpose of this operation?"

I think that the most important aspect of that code is to understand that the multiplication by 4000 is really a division.

What ! but it says _builtin_mulss(y_rate, 4000); Surely you say that is multiplication ?? Yes it is.

But look what happens in the next line .....

udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

Note the use of ._.W1

._.W1 means that only the top 16 bits of a 32 bit word is going to be used. The bottom 16 bits of the 32 bit word are dropped.

The net result is that we have really multiplied by the fraction of (4000  / 65536) which is 0.061 .  

As to the original logic for using exactly 4000 (or 0.061), that I cannot relate. But it is going to be a ratio which takes either the accelerometer value,

or the gyro value, makes it smaller, and when it added onto 3000, probably (I say probably because I have not done the detail calculations), will give a good value of between +1000 to -1000 for expected values of accel and gyros.

3000+1000 = 4000 which is maximum Servo deflection in UDB Units.  UDB units are twice the microsecond pulse length. (4000 / 2 which is 2000 for maximum servo deflection)(3000 / 2 being 1500 micro seconds)..

Bill may remember the original reason for using 4000 in more detail.

I hope that helps.

Leonardo, it is super to see you making progress. Thanks for the updates.

Best wishes, Pete

--
You received this message because you are subscribed to the Google Groups "uavdevboard-dev" group.
To unsubscribe from this group and stop receiving emails from it, send an email to uavdevboard-d...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[William Premerlani]

Peter and Leonardo,

The multiplication of the sensor values by 4000 followed by division by 65536 maps the range of sensor values to the range of PWM values.

The logic of that is as follows.

There is a divide by 2 in the firmware that measures the sensors, so the sensor range is +-32768/2, or +- 65536/4. When you divide that by 65536 and multiply by 4000, the result is in the range of +-1000. So, the code being discussed maps the full range of sensor values to the full range of the servos connected to those channels.

Best regards,

Bill

[Leonardo Garberoglio]

Peter, Bill thank you very much for your fast answer!! I understand the code now and begin the port to stm.
BTW there is no scaling for raw gyro and accel data? If I choose 2g or 4g con mpu init, how it is computed? I couldn't find scaling code...
U will update my progress once I have led test running on new hardware system.

Bw, Leo

[William Premerlani]

Leonardo,

Do a search for SCALEGYRO and SCALEACCEL, they define the gyro and accelerometer scale factors.

They are not used for the LED test, since for that the scaling does not matter.

They are used in all of the other firmware projects, such as MatrixPilot and also the roll pitch yaw demo, because there the sensor values are used in computations.

Best regards,

Bill

[Leonardo Garberoglio]

Thank Bill, I could find the definition on ConfigUDB5.h:
#define ACCEL_RANGE         4       // 4 g range

// note : it is possible to use other accelerometer ranges on the MPU6000
#define SCALEGYRO           3.0016  // 500 degree/second range
#define SCALEACCEL          1.29    // 4 g range

BW!

Leo