PIC32MK MC QEI example

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
Added a new display type: DOGXL240-7 https://www.lcd-module.com/fileadmin/eng/pdf/grafik/dogxl240-7e.pdf
A lot more real-estate. I built another display board with the back-light and led current driver but you don't need it in normal light conditions.
Same 8MHz SPI interface and basic driver guts.

Low speed motor application display on the PIC32MKMC board running the Teknic AC servo IPMSM motor with 3 phase 0.066 millihertz (0.001 hertz) sinewave drive signals using encoder feedback for speed (QEI clock timer counts between edges) control and positioning.
https://www.ti.com/store/ti/en/p/product/?p=LVSERVOMTR

 

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
SAPWM (Saddle PWM) waveform theory and application.

https://www.infineon.com/dgdl/AP160...M.pdf?fileId=db3a304412b407950112b40a1bf20453

https://ieeexplore.ieee.org/document/5712490
Improved inverter utilisation using third harmonic injection
C. Generation of Switching Signals
The general block diagram for generation of the switching pulses is shown in the fig. 6(a). The reference voltage Vref is added with signal having frequency three times of fundamental frequency and the magnitude is 1/6th of the fundamental amplitude. The resultant is then passed through comparator which compares the modified signal with the carrier signal of frequency 2 KHz. In the fig. 6(b) the combination of all the three signal is been shown.
Here I just generate a static sin table of the needed waveform when the program starts that's used later in the software for the PWM modulation index.
\( y=\sin \theta+{1\over 6}\sin\, 3\theta \)
C:
void sine_table(void)
{
    int I, H, j = 1;

    for (I = 0; I < sine_res; I++) {
        sine_const[I] = sin((M_PI * 2.0 * (double) I) / (double) sine_res);
        H = I * HARMONIC_NUM; // need third harmonic
        if (H >= sine_res * j) {
            H = H - sine_res*j;
            j++;
        }
        /*
         * compute saddle-back waveform instead of pure sine
         * THA sets the harmonic content, 1/6 is the recommended amount but we use more here
         */
        sine_const[I] = sine_const[I] + ((sin((M_PI * 2.0 * (double) H) / (double) sine_res)) * THA);
    }
}

O-scope RC filters for PWM motor drive signal.


FFT of fundamental and third harmonic content.

PWM phase signal (filtered to remove the pwm carrier) waveform. One of three phase shifted generated from the sine_const table.




Two pwm phase shifted signal waveforms (top/bottom) and the center MATH line waveform (clean sine-wave) that drives a set of motor windings on the 3-ph motor.
 
Last edited:

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
LOL

Working with a new chip: https://www.microchip.com/wwwproducts/en/PIC32MK0512MCJ064
On a new dev board: http://ww1.microchip.com/downloads/...MCJ-Curiosity-Pro-Users-Guide-DS70005421A.pdf

Nice board but something is wrong.

I'm testing some SPI interface code to a Parallax LSM9DS1 9-axis IMU Module using the PIC32MK MCJ Curiosity Pro Development board (DT100113) with the PIC32MK0512MCJ064. So far the hardware is solid but some numb-skull put the Arduino Uno digital header pins backwards on the DT100113 PCB. The labels are right but none of the electrical pins go to the correct place.

LOL The PCB header pins should start from the bottom and work up per the label and standard shield connections.

As you can see the IMU module uses 3-pin SDI half-duplex so the MISO and MOSI are shorted together at the controller board with a single data wire to the module.

The setup for 3-wire SPI on the PIC32MK is simple. You can change the SPI TX pin function when needed to a input PORT pin function.
C:
int imu_init(int pinSCL, int pinSDIO, int pinAG, int pinM)
{
    __pinAG = pinAG;
    __pinM = pinM;
    __pinSDIO = pinSDIO;
    __pinSCL = pinSCL;

    // Set both the Accel/Gyro and Mag to 3-wire SPI mode
    /*
     * We don't use SPI bit-banging for 3-wire because it's possible to setup a hardware 3-wire mode on the PIC32 module
     * on the pic32mk hardware SPI module jumper mosi to miso for half-duplex 3-wire mode and config for SPI MODE 3
     * set the TRIS mode on the SPI TX port pin for input so we can turn-off the transmitter voltage during SPI receive
     * on the bidirectional SPI line
     *
     * SPIxCON: SPI CONTROL REGISTER
     * bit 12 DISSDO: Disable SDOx pin bit(4)
     * 1 = SDOx pin is not used by the module. Pin is controlled by associated PORT register
     * 0 = SDOx pin is controlled by the module
     */
    TRISCbits.TRISC7 = 1; // SPI2 TX port set to port control as a input so we don't affect received data when in port mode
    imu_SPIwriteByte(__pinAG, CTRL_REG8, 0b00001100);
    imu_SPIwriteByte(__pinM, CTRL_REG3_M, 0b10000100);

    // To verify communication, we can read from the WHO_AM_I register of
    // each device. Store those in a variable so we can return them.
    char xgTest = 0, mTest = 0;
    imu_SPIreadBytes(__pinM, WHO_AM_I_M, (unsigned char *) &mTest, 1); // Read the gyro WHO_AM_I
    imu_SPIreadBytes(__pinAG, WHO_AM_I_XG, (unsigned char *) &xgTest, 1); // Read the accel/mag WHO_AM_I
    int whoAmICombined = (xgTest << 8) | mTest;

    if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP)) return 0;

    //Init Gyro
    imu_SPIwriteByte(__pinAG, CTRL_REG1_G, 0xC0);
    imu_SPIwriteByte(__pinAG, CTRL_REG2_G, 0x00);
    imu_SPIwriteByte(__pinAG, CTRL_REG3_G, 0x00);
    imu_SPIwriteByte(__pinAG, CTRL_REG4, 0x38);
    imu_SPIwriteByte(__pinAG, ORIENT_CFG_G, 0x00);

    //Init Accel
    imu_SPIwriteByte(__pinAG, CTRL_REG5_XL, 0x38);
    imu_SPIwriteByte(__pinAG, CTRL_REG6_XL, 0xC0);
    imu_SPIwriteByte(__pinAG, CTRL_REG7_XL, 0x00);

    //Init Mag
    imu_SPIwriteByte(__pinM, CTRL_REG2_M, 0x00);
    imu_SPIwriteByte(__pinM, CTRL_REG4_M, 0x0C);
    imu_SPIwriteByte(__pinM, CTRL_REG5_M, 0x00);

    //Set Scales
    imu_setGyroScale(500);
    imu_setAccelScale(8);
    imu_setMagScale(12);

    //Look for calibrations in EEPROM
    char biasStamp[7] = {0};
    char mBiasStored[7] = {0};
    char aBiasStored[7] = {0};
    char gBiasStored[7] = {0};
    //  i2c_in(eeBus, 0b1010000, 63280, 2, biasStamp, 7);
    //  i2c_in(eeBus, 0b1010000, 63287, 2, mBiasStored, 7);
    //  i2c_in(eeBus, 0b1010000, 63294, 2, aBiasStored, 7);
    //  i2c_in(eeBus, 0b1010000, 63301, 2, gBiasStored, 7);

    if (strcmp(biasStamp, "LSM9DS1") == 0) {
        if ((mBiasStored[0] = 'm')) {
            int mxB = (mBiasStored[1] << 8) | mBiasStored[2];
            int myB = (mBiasStored[3] << 8) | mBiasStored[4];
            int mzB = (mBiasStored[5] << 8) | mBiasStored[6];

            imu_setMagCalibration(mxB, myB, mzB);
        }

        if ((aBiasStored[0] = 'a')) {
            int axB = (aBiasStored[1] << 8) | aBiasStored[2];
            int ayB = (aBiasStored[3] << 8) | aBiasStored[4];
            int azB = (aBiasStored[5] << 8) | aBiasStored[6];

            imu_setAccelCalibration(axB, ayB, azB);
        }

        if ((gBiasStored[0] = 'g')) {
            int gxB = (gBiasStored[1] << 8) | gBiasStored[2];
            int gyB = (gBiasStored[3] << 8) | gBiasStored[4];
            int gzB = (gBiasStored[5] << 8) | gBiasStored[6];

            imu_setGyroCalibration(gxB, gyB, gzB);
        }
    }



    // Once everything is initialized, return the WHO_AM_I registers we read:
    return whoAmICombined;
}
C:
/*
* PIC32: changed to use hardware SPI module and plib_gpio pin functions
* IMU SPI is half-duplex so we short pic32 SPI TX to RX signals and toggle TX pin state to PORT pin mode input during SPI receive
*/
void imu_SPIreadBytes(unsigned char csPin, unsigned char subAddress, unsigned char *dest, unsigned char count)
{
    int i;

    // To indicate a read, set bit 0 (msb) of first byte to 1
    unsigned char rAddress = 0x80 | (subAddress & 0x3F);

    // Mag SPI port is different. If we're reading multiple bytes,
    // set bit 1 to 1. The remaining six bytes are the address to be read
    if ((csPin == __pinM) && count > 1) rAddress |= 0x40;

    SPI2CONbits.DISSDO = 0; // half-duplex switching, SPI TX ON
    if (csPin == __pinM)
        csPin_m_Clear();
    else
        csPin_ag_Clear();

    SPI2_Write(&rAddress, 1);
    SPI2CONbits.DISSDO = 1;
    for (i = 0; i < count; i++) {
        SPI2_Read(&dest[i], 1); // half-duplex switching, SPI TX OFF, PORT pin mode input
    }
    if (csPin == __pinM)
        csPin_m_Set();
    else
        csPin_ag_Set();

}
Inside the read function we switch TX pin modes from TX (send address to read) DISSDO=0 to receive with DISSDO=1 so the data sent by the module doesn't get clobbered by the uC bits on the single wire when we read back data.



The data at SPI 8MHz clocks during bytes 6 & 8 are responses from the module for the init device queries.

C:
    imu_SPIreadBytes(__pinM, WHO_AM_I_M, (unsigned char *) &mTest, 1); // Read the gyro WHO_AM_I
    imu_SPIreadBytes(__pinAG, WHO_AM_I_XG, (unsigned char *) &xgTest, 1); // Read the accel/mag WHO_AM_I
Once these are read and checked the rest of the init sequence of spi data exchanges can continue.

What this is for?

https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
 
Last edited:

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
Step one is done. I can move the cube on the Linux computer screen Java program via a USB serial comm port on the MCJ board. o_O

Lot's of FP ops in the sensor fusion function.
https://www.allaboutcircuits.com/te...lost-in-deep-space-understanding-quaternions/
45:00 for a physical rotation explanation.
C:
void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float hx, hy;
    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
        return;
    }

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;

        // Normalise magnetometer measurement
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;

        // Auxiliary variables to avoid repeated arithmetic
        _2q0mx = 2.0f * q0 * mx;
        _2q0my = 2.0f * q0 * my;
        _2q0mz = 2.0f * q0 * mz;
        _2q1mx = 2.0f * q1 * mx;
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _2q0q2 = 2.0f * q0 * q2;
        _2q2q3 = 2.0f * q2 * q3;
        q0q0 = q0 * q0;
        q0q1 = q0 * q1;
        q0q2 = q0 * q2;
        q0q3 = q0 * q3;
        q1q1 = q1 * q1;
        q1q2 = q1 * q2;
        q1q3 = q1 * q3;
        q2q2 = q2 * q2;
        q2q3 = q2 * q3;
        q3q3 = q3 * q3;

        // Reference direction of Earth's magnetic field
        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
        _2bx = sqrt(hx * hx + hy * hy);
        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
        _4bx = 2.0f * _2bx;
        _4bz = 2.0f * _2bz;

        // Gradient decent algorithm corrective step
        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * (1.0f / sampleFreq);
    q1 += qDot2 * (1.0f / sampleFreq);
    q2 += qDot3 * (1.0f / sampleFreq);
    q3 += qDot4 * (1.0f / sampleFreq);

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}
It takes 104us total with 14us processing time (32-bit hardware floating point is great) for a complete position update but the magnetometer 80 Hz refresh rate is the limiting factor. I'm only sampling at 10 Hz.


Just need to clean this up wire-jungle into a eagle-cad package for a compatible plugin board.

 
Last edited:

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
Next step is to extract linear acceleration from gravity in the accelerometer. We normally use gravity as an orientation pointer to earth that is mixed into the quaternion sensor fusion variable. First calculate the gravity vector from the quaternion with a rotation to earth. Then you normalize the accelerometer data readings to 1 G and simply subtract the accel vector from the gravity vector giving only linear acceleration as the result (a raw result that drifts without processing).
https://en.m.wikipedia.org/wiki/Rot...Quaternion_→_Euler_angles_(z-y′-x″_intrinsic)

Now I can bounce (add the accel vector to the cube display origin 0,0,0 vector) the cube and rotate it.

Made a sensor 'Hat' for the dev board. The stupid design error on the Microchip PCB required the removal of the ground connection on one pin of a standard shield proto board but the rest were OK being connected flipped around on the digital side.
 

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
Added some NVRAM code using a program flash page to save calibration data and ported the DOGXL240-7 LCD driver to the new hardware.

C:
    /*
     * talk to the parallax LSM9DS1 9-axis IMU
     */

    UART1_Write((uint8_t *) cbuffer, strlen(cbuffer));
    if (!imu_init(1, 2, 3, 4)) {
        // Trouble in River-City, not talking to the IMU
        UART1_Write((uint8_t *) imu_missing, strlen(imu_missing));
        sprintf(buffer, "IMU SPI init error.");
        eaDogM_WriteStringAtPos(2, 0, buffer);
        OledUpdate();
    } else {
        sprintf(buffer, "IMU SPI init complete.");
        eaDogM_WriteStringAtPos(2, 0, buffer);
        OledUpdate();
        if (!SWITCH_Get()) {
            sprintf(buffer, "Erasing IMU CAL data.     ");
            eaDogM_WriteStringAtPos(9, 0, buffer);
            OledUpdate();
            NVMerase_page(); // erase the calibrations data page
            sprintf(buffer, "IMU calibration starting.");
            eaDogM_WriteStringAtPos(3, 0, buffer);
            OledUpdate();
            imu_calibrateAG(); // write new cal data
            sprintf(buffer, "IMU AG done. Starting Mag");
            eaDogM_WriteStringAtPos(4, 0, buffer);
            OledUpdate();
            imu_calibrateMag(); // write new cal data
            sprintf(buffer, "IMU Mag cal done. IMU ready.");
            eaDogM_WriteStringAtPos(5, 0, buffer);
            OledUpdate();
        }
    };
Display SPI (tx only) uses one of seven DMA transfer modules.

The graphics screen buffer of 3840 bytes is updated in parallel of main program execution in 4.48ms.


Using the enhanced SPI module with FIFO enabled and DMA it's possible to have no gaps in data transfers between bytes.
The display uses this mode for screen updates.


With a small change to the default SPI configuration it's possible to insert a small delay between bytes for devices that need it.
C:
/* 
 * File:   display_type.h
 * Author: root
 *
 * Created on August 18, 2020, 3:04 PM
 */

#ifndef DISPLAY_TYPE_H
#define    DISPLAY_TYPE_H

#ifdef    __cplusplus
extern "C" {
#endif

    /*
     * use DMA for display buffer updates
     * configure SPI for no interrupts. DMA triggers on the SPITX flag bit
     */
#define USE_DMA
#ifndef USE_DMA
    /*
     * using the interrupt version of the harmony plib SPI driver, 1/4 pure cpu speed
     * undefine USE_INT for the pure cpu version
     */
    //#define USE_INT
#endif

    //Select the display type: DOGS102: 102, DOGM128/DOGL128: 128, DOGM132: 132, DOGXL160: 160, DOGXL240: 240
#define DISPLAY_TYPE  240
    //Display Orientation: Normal (0) or upside-down (1)?
#define ORIENTATION_UPSIDEDOWN 0

    //Should chip select (CS) be used?
#define LCD_USE_CHIPSELECT  1

    //Use Backlight?  (0: no backlight, 1: backlight (on when pin is high), 2: backlight (on when pin is low))
#define LCD_USE_BACKLIGHT   1
    //#define EDOGM
#define EDOGS
    //#define EDOGS_DEMO


#include "definitions.h"                // SYS function prototypes
#include "config/pic32mk_mcj_curiosity_pro/peripheral/spi/spi_master/plib_spi1_master.h"
#include "config/pic32mk_mcj_curiosity_pro/peripheral/../peripheral/gpio/plib_gpio.h"
#include "config/pic32mk_mcj_curiosity_pro/peripheral/dmac/plib_dmac.h"

    extern void RS_SetLow(void);
    extern void RS_SetHigh(void);
    extern void CSB_SetLow(void);
    extern void CSB_SetHigh(void);
    extern void SPI_Exchange8bit(uint8_t);
    extern void SPI_ExchangeBuffer(uint8_t *, uint16_t);
    extern uint16_t SPI_to_Buffer(uint8_t *, uint16_t, uint8_t *);
    void wait_lcd_done(void);

#ifdef    __cplusplus
}
#endif

#endif    /* DISPLAY_TYPE_H */
C:
#include "display_type.h"
#include "dogm-graphic.h"

void RS_SetLow(void)
{
    wait_lcd_done();
    RS_Clear(); // display board SPI_EN0
};

void RS_SetHigh(void)
{
    wait_lcd_done();
    RS_Set();
};

void CSB_SetLow(void)
{
    wait_lcd_done();
    CSB_Clear(); // display board SPI_EN1
};

void CSB_SetHigh(void)
{
    wait_lcd_done();
    CSB_Set();
};

void SPI1DmaChannelHandler(DMAC_TRANSFER_EVENT event, uintptr_t contextHandle)
{
    if (event == DMAC_TRANSFER_EVENT_COMPLETE) {
        LCD_UNSELECT();
    }
}

void SPI_Exchange8bit(uint8_t data)
{
#ifdef USE_INT
    while (SPI1_IsBusy());
#endif
#ifdef USE_DMA
    while (DMAC_ChannelIsBusy(DMAC_CHANNEL_0));
#endif
    SPI1_Write(&data, 1);
};

void SPI_ExchangeBuffer(uint8_t *data, uint16_t len)
{
#ifdef USE_INT
    while (SPI1_IsBusy());
#endif
#ifdef USE_DMA
    while (DMAC_ChannelIsBusy(DMAC_CHANNEL_0));
#endif
    SPI1_Write(data, len);
};

uint16_t SPI_to_Buffer(uint8_t *dataIn, uint16_t bufLen, uint8_t *dataOut)
{
    uint16_t bytesWritten = 0;

#ifdef USE_DMA
    while (DMAC_ChannelIsBusy(DMAC_CHANNEL_0));
    DMAC_ChannelCallbackRegister(DMAC_CHANNEL_0, SPI1DmaChannelHandler, 0);
    SPI1CONbits.STXISEL = 1; // set to 0 for byte gaps
    SPI1CONbits.ENHBUF = 1; // enable FIFO
    bytesWritten = bufLen;
    LCD_SELECT();
    LCD_DRAM();
    if (bufLen != 0) {
        DMAC_ChannelTransfer(DMAC_CHANNEL_0, (const void *) dataIn, (size_t) bufLen, (const void*) &SPI1BUF, (size_t) 1, (size_t) 1);
    }
    return bytesWritten;
#else
#ifdef USE_INT
    while (SPI1_IsBusy());
#endif

    LCD_SELECT();
    LCD_DRAM();
    if (bufLen != 0) {
#ifdef EDOGS
        SPI_ExchangeBuffer(dataIn, bufLen);
        bytesWritten = bufLen;
#endif
#ifdef EDOGM
        if (dataIn != NULL) {
            while (bytesWritten < bufLen) {
                if (dataOut == NULL) {
                    SPI_Exchange8bit(dataIn[bytesWritten]);
                } else {
                    SPI_Exchange8bit(dataIn[bytesWritten]);
                }
                lcd_inc_column(1);
                bytesWritten++;
            }
        } else {
            if (dataOut != NULL) {
                while (bytesWritten < bufLen) {
                    SPI_Exchange8bit(0xff);
                    lcd_inc_column(1);
                    bytesWritten++;
                }
            }
        }
#endif
    }
    LCD_UNSELECT();
    return bytesWritten;
#endif
}

void wait_lcd_done(void)
{
#ifdef USE_DMA
    while (DMAC_ChannelIsBusy(DMAC_CHANNEL_0));
#else
#ifdef USE_INT
    while (SPI1_IsBusy());
#endif
#endif
}
9-axis calibration saved to NVRAM
 

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
It's a DOGS102-6 LCD (102x64 PIXELS) that uses the UC1701x 65x132 STN LCD Controller-Driver.
https://www.lcd-module.de/eng/pdf/zubehoer/uc1701.pdf

The other graphics display is a DOGXL240-7 with a UC1611S 160COM x 256SEG Matrix LCD Controller-Driver w/ 16-shade per pixel but that's on the back burner.


[/code]
PXL_20210331_150932614.jpg
Upgraded the pic32mk to the second-generation MCM series on the motor control boards. 4 CAN Fd peripheral, Motor Control special features (QEI and enhanced PWM) and ECC flash.
https://www.microchip.com/wwwproducts/en/PIC32MK1024MCM100
 

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
A memory to memory PIC32MK MCM DMA example to show how optimizing routines to utilize hardware can increase speed and efficiency in controller projects.

We can use a typical C software loop to clear a section of graphic memory or use one channel of the controllers DMA hardware for the same set of functions.

C code fragments
C:
#define    cbOledDispMax        3840        // max number of bytes in display buffer

/* This array is the offscreen frame buffer used for rendering.
 ** It isn't possible to read back frome the OLED display device,
 ** so display data is rendered into this offscreen buffer and then
 ** copied to the display.
 *  must be in uncached memory for pic32 DMA so use __attribute__((coherent))
 * DMA0 SPI TX transfers
 * DMA1 GLCD buffer transfers
 */

uint8_t __attribute__((coherent)) rgbOledBmp0[cbOledDispMax]; // two display buffers for page flipping
uint8_t __attribute__((coherent)) rgbOledBmp1[cbOledDispMax];
uint8_t __attribute__((coherent)) rgbOledBmp_blank[4]; // 32-bit frame-buffer clearing variable

/* just clears a output timing flag bit */
void CBDmaChannelHandler(DMAC_TRANSFER_EVENT event, uintptr_t contextHandle)
{
    if (event == DMAC_TRANSFER_EVENT_COMPLETE) {
        DEBUGB0_Clear(); // clear timing flag bit
    }
}

void OledClearBuffer(void)
{
    uint8_t * pb;

    DEBUGB0_Set(); // set timing flag bit for o-scope
    if (disp_frame) {
        pb = rgbOledBmp0;
    } else {
        pb = rgbOledBmp1;
    }

#define USE_DMAok
#ifdef USE_DMAok
    while (DMAC_ChannelIsBusy(DMAC_CHANNEL_1)); // wait for possible DMA1 transfer to complete
    DMAC_ChannelCallbackRegister(DMAC_CHANNEL_1, CBDmaChannelHandler, 0); // end of transfer interrupt function
    /* setup the source and destination parms */
    DMAC_ChannelTransfer(DMAC_CHANNEL_1, (const void *) rgbOledBmp_blank, (size_t) 4, (const void*) pb, (size_t) cbOledDispMax, (size_t) cbOledDispMax);
    DCH1ECONSET = 0x00000080; // set CFORCE to 1 to start the transfer
#else
    int32_t ib;

    /* Fill the memory buffer with 0.
     */
    for (ib = 0; ib < cbOledDispMax; ib++) {
        *pb++ = 0x00;
    }
    DEBUGB0_Clear();
#endif
Software:
Mainline code is stuck burning cpu time for this amount of time while the routine completes.

DMA hardware:
The total time to complete is much quicker and mainline code only does the setup and trigger, it doesn't wait for completion.


https://ww1.microchip.com/downloads/en/DeviceDoc/60001117H.pdf
 

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
A memory to SPI PIC32MK MCM DMA. Driving a GLCD display usually requires page/address commands and buffer data writes to update the display from a memory buffer that routines use to draw to the screen.

A generic example: If USE_DMA and DMA_STATE_M are not defined the CPU to SPI version of the driver is used.
C:
void OledUpdate(void)
{
#ifdef DMA_STATE_M
    wait_lcd_done();
    SPI3DmaChannelHandler_State(0, DMA_MAGIC); // set DMA state machine init mode to start transfers
    return;
#endif
#ifdef EDOGS
    int32_t ipag;
    uint8_t* pb;

    DEBUGB0_Set();
    if (disp_frame) {
        pb = rgbOledBmp0;
    } else {
        pb = rgbOledBmp1;
    }
    rgbOledBmp_page[4] = 0;

    for (ipag = 0; ipag < cpagOledMax; ipag++) { // mainline code loop for GLCD update
        /* Set the page address
         */
        //Set page command
        //page number
        /* Start at the left column
         */
        //set low nibble of column
        //set high nibble of column
        lcd_moveto_xy(ipag, 0);
        /* Copy this memory page of display data.
         */
        OledPutBuffer(ccolOledMax, pb);
        pb += ccolOledMax;
    }
    DEBUGB0_Clear();
#endif
}

The top Yellow trace is time in the mainline execution thread during the screen update. HIGH means CPU working on SPI transfer.
The Purple trace is 8MHz SPI clocks.

Pure CPU is fast but it blocks (yellow trace is HIGH) user code execution until it completes and there are sometimes gaps when sending single byte commands/data.
Interrupt driven SPI could be used but it's usually slower and still has CPU usage during actual I/O transfers.

With USE_DMA and DMA_STATE_M defined all I/O is handled by the DMA controller and page/addressing commands are handled in the DMA event ISR state machine.
C:
/*
* start a GLCD update: Called in user code with contextHandle set to DMA_MAGIC for a background screen update,
* during background transfers this function is used as the callback for DMA transfer complete events to
* sequence commands and data to the GLCD via the SPI port using the dstate ENUM variable
* dstate is set to 'D_idle' when the complete set of transfers is done.
*/
void SPI3DmaChannelHandler_State(DMAC_TRANSFER_EVENT event, uintptr_t contextHandle)
{
    static int32_t ipag; // buffer page number
    static uint8_t* pb; // buffer page address

    DEBUGB0_Set(); // back to mainline code, GLCD updates in background using DMA and interrupts
    LCD_UNSELECT();
    if (contextHandle == DMA_MAGIC) { // re-init state machine for next GLCD update
        dstate = D_init;
    }

    switch (dstate) {
    case D_init:
        ipag = 0;
        if (disp_frame) { // select flipper buffer
            pb = rgbOledBmp0;
        } else {
            pb = rgbOledBmp1;
        }
    case D_page: // send the page address commands via DMA
        LCD_SELECT(); // enable the GLCD chip for SPI transfers
        dstate = D_buffer;
        lcd_moveto_xy(ipag, 0);  // calculate address data nibbles and store in rgbOledBmp_page array
        /*
         * DMAC_ChannelCallbackRegister and SPI setup in OledInit
         */
        LCD_CMD();
        DMAC_ChannelTransfer(DMAC_CHANNEL_0, (const void *) rgbOledBmp_page, (size_t) 4, (const void*) &SPI3BUF, (size_t) 1, (size_t) 1);
        break;
    case D_buffer: // send the GLCD buffer data via DMA
        ipag++;
        if (ipag <= cpagOledMax) {
            LCD_SELECT(); // enable the GLCD chip for SPI transfers
            dstate = D_page;
            LCD_DRAM();
            DMAC_ChannelTransfer(DMAC_CHANNEL_0, (const void *) pb, (size_t) ccolOledMax, (const void*) &SPI3BUF, (size_t) 1, (size_t) 1);
            pb += ccolOledMax;
        } else {
            dstate = D_idle;
            LCD_UNSELECT(); // all done with the GLCD
        }
        break;
    case D_idle:
    default:
        LCD_UNSELECT();
        break;
    }
    DEBUGB0_Clear();
}

The top yellow trace is time in the mainline execution thread during the screen update. HIGH means CPU working on SPI transfer.

Now the only time the CPU executes for SPI transfers is the short time it spends in the event interrupt code setting up the GLCD page/address DMA transfer and then the DMA transfer setup of the actual page data. That's only several microseconds of time away from mainline code thread execution per every SPI block transfer.


>60Hz possible refresh rate with minimal CPU and Interrupt usage during the SPI transfers.
 
Last edited:

cmartinez

Joined Jan 17, 2007
8,218
A memory to SPI PIC32MK MCM DMA. Driving a GLCD display usually requires page/address commands and buffer data writes to update the display from a memory buffer that routines use to draw to the screen.

A generic example: If USE_DMA and DMA_STATE_M are not defined the CPU to SPI version of the driver is used.
C:
void OledUpdate(void)
{
#ifdef DMA_STATE_M
    wait_lcd_done();
    SPI3DmaChannelHandler_State(0, DMA_MAGIC); // set DMA state machine init mode to start transfers
    return;
#endif
#ifdef EDOGS
    int32_t ipag;
    uint8_t* pb;

    DEBUGB0_Set();
    if (disp_frame) {
        pb = rgbOledBmp0;
    } else {
        pb = rgbOledBmp1;
    }
    rgbOledBmp_page[4] = 0;

    for (ipag = 0; ipag < cpagOledMax; ipag++) { // mainline code loop for GLCD update
        /* Set the page address
         */
        //Set page command
        //page number
        /* Start at the left column
         */
        //set low nibble of column
        //set high nibble of column
        lcd_moveto_xy(ipag, 0);
        /* Copy this memory page of display data.
         */
        OledPutBuffer(ccolOledMax, pb);
        pb += ccolOledMax;
    }
    DEBUGB0_Clear();
#endif
}

The top Yellow trace is time in the mainline execution thread during the screen update. HIGH means CPU working on SPI transfer.
The Purple trace is 8MHz SPI clocks.

Pure CPU is fast but it blocks (yellow trace is HIGH) user code execution until it completes and there are sometimes gaps when sending single byte commands/data.
Interrupt driven SPI could be used but it's usually slower and still has CPU usage during actual I/O transfers.

With USE_DMA and DMA_STATE_M defined all I/O is handled by the DMA controller and page/addressing commands are handled in the DMA event ISR state machine.
C:
/*
* start a GLCD update: Called in user code with contextHandle set to DMA_MAGIC for a background screen update,
* during background transfers this function is used as the callback for DMA transfer complete events to
* sequence commands and data to the GLCD via the SPI port using the dstate ENUM variable
* dstate is set to 'D_idle' when the complete set of transfers is done.
*/
void SPI3DmaChannelHandler_State(DMAC_TRANSFER_EVENT event, uintptr_t contextHandle)
{
    static int32_t ipag; // buffer page number
    static uint8_t* pb; // buffer page address

    DEBUGB0_Set(); // back to mainline code, GLCD updates in background using DMA and interrupts
    if (contextHandle == DMA_MAGIC) { // reinit state machine for next GLCD update
        dstate = D_init;
    }

    switch (dstate) {
    case D_init:
        ipag = 0;
        if (disp_frame) { // select flipper buffer
            pb = rgbOledBmp0;
        } else {
            pb = rgbOledBmp1;
        }
        LCD_SELECT(); // enable the GLCD chip for SPI transfers
    case D_page: // send the page address commands via DMA
        dstate = D_buffer;
        lcd_moveto_xy(ipag, 0);
        /*
         * DMAC_ChannelCallbackRegister and SPI setup in OledInit
         */
        LCD_CMD();
        DMAC_ChannelTransfer(DMAC_CHANNEL_0, (const void *) rgbOledBmp_page, (size_t) 4, (const void*) &SPI3BUF, (size_t) 1, (size_t) 1);
        break;
    case D_buffer: // send the GLCD buffer data via DMA
        ipag++;
        if (ipag <= cpagOledMax) {
            dstate = D_page;
            LCD_DRAM();
            DMAC_ChannelTransfer(DMAC_CHANNEL_0, (const void *) pb, (size_t) ccolOledMax, (const void*) &SPI3BUF, (size_t) 1, (size_t) 1);
            pb += ccolOledMax;
        } else {
            dstate = D_idle;
            LCD_UNSELECT(); // all done with the GLCD
        }
        break;
    case D_idle:
    default:
        LCD_UNSELECT();
        break;
    }
    DEBUGB0_Clear();
}

The top yellow trace is time in the mainline execution thread during the screen update. HIGH means CPU working on SPI transfer.

Now the only time the CPU executes for SPI transfers is the short time it spends in the event interrupt code setting up the GLCD page/address DMA transfer and then the DMA transfer setup of the actual page data. That's only several microseconds of time away from mainline code thread execution per every SPI block transfer.


>60Hz possible refresh rate with minimal CPU and Interrupt usage during the SPI transfers.
I've always thought that a scope's graph is a thing of beauty... especially when everything works according to one's desire... :)
 

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
I've always thought that a scope's graph is a thing of beauty... especially when everything works according to one's desire... :)
Scopes are great but they need to be told where to look. You need to know where and when the gpio debug pin on the trigger channel needs to toggle.

A El Cheapo logic probe works just fine too.
 
Last edited:

Thread Starter

nsaspook

Joined Aug 27, 2009
13,079
Using the RS485 functionality of the PIC32MK board to communicate to a 3-phase energy monitor with the MODBUS protocol.

Testing a few boat-anchor power 40V 125A DC supplies before they are scrapped.
PXL_20220504_233659172.jpgPXL_20220504_233753344.jpgPXL_20220504_233758871.jpgPXL_20220504_234042573.jpgPXL_20220504_234058611.jpgPXL_20220504_234239967.jpg
https://www.gavazzionline.com/pdf/EM540_DS_ENG.pdf

The PIC32MK board software runs a simple MODBUS master (instead of the normal monitor MODBUS client) to poll the power monitor for three-phase voltage, current and power while still handling the controllers inverter three-phase power generation functions.

https://github.com/nsaspook/vcan/blob/em540_modbus/firmware/src/modbus_master.h
https://github.com/nsaspook/vcan/blob/em540_modbus/firmware/src/modbus_master.c
 
Top