Embedded World 2021 - Infineon ModusToolbox PSoC 6 Drone 

 

Summary

In lesson 6 we will turn our attention to the drone. Everything on the drone side is almost exactly the same as remote control, so I will copy all of that code to start with then add the motor controller.  At the end of lesson one you will have the functioning drone controller.  Here is the architecture:

Learning Objectives

  1. Retargeting a project to a different board
  2. Using the Infineon BLDC Motor Controller
  3. Subscribing to a MQTT topic

Procedure

  1. Make a new project
  2. Rip out the joystick_task.c/h and fix main.c
  3. Add the motor driver library
  4. Create motor_task.h
  5. Create motor_task.c
  6. Update the CapSense task to submit to the motor_task
  7. Fix the Cloud Task to perform MQTT subscribe instead of publish
  8. Fix main.c to start the motor task
  9. Test

1. Make a new project

In order to control the BLDC motor I am going to a development kit that has Arduino headers, the CY8CKIT-062S2-43012.  Start your project from that BSP.

Our project is basically the same as Lesson4, so import that as a template.

Don’t sully your project’s name, pick something good.

2. Rip out the joystick_task

This project will not have the joystick task.  Nuke it! Don’t forget to remove the task start from main.c

3. Add the Motor Driver Library and Remove the Magnetic Sensor

For this project we will be talking to the TLE9879 BLDC motor shield.  I provide you a driver library for that.  And we won’t need the 3-D Magnetic sensing library any more.

4. Create motor_task.h

Create a motor_task.h  This will have the definition of the task, plus a function to set the motor speed by submitting to a queue.

#pragma once

void motor_task(void* param);

void motor_update(int speed);

5. Create motor_task.c

The motor task needs some include files.  This task will also use a queue to manage other tasks (cloud and CapSense) that will submitting data to change the speed of the motor.

#include "cybsp.h"
#include "cyhal.h"
#include "cycfg.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "motor_task.h"

#include "tle9879_system.h"

static QueueHandle_t motor_value_q;

I will define a few constants that will help me map 0-100% into 0-5500 RPM (the max for this motor). The motor needs to run at a decent speed to start up properly so we will map 10% to 1000 RPM. Below 10%, the motor will be off.

/*******************************************************************************
* Global constants
*******************************************************************************/
#define		RPM_CHANGE_INTERVAL		(100)
#define		RPM_CHANGE_RATE			(10)
#define		RPM_PERCENT_MIN			(10)

#define  	RPM_MAX 				5500.0
#define  	RPM_MIN 				1000.0

The task is pretty simple, but with one trick.  Apparently you ramp changes in motor speed, so I use a timeout in the message queue to give me a periodic change.  The task is

  1. Start the shield
  2. Then wait for a new value in the queue (or the timeout)
  3. If it is a new value then change the desired value
  4. If the current value is not the same as the desired value then take a step towards the desired value
void motor_task(void* param)
{
    (void)param;

    tle9879_sys_t tle9879_sys;

    uint8_t numberOfBoards = 1;
    BaseType_t rtos_api_result;

    motor_value_q = xQueueCreate(1,sizeof(int));

    /* Initialize and configure the motor driver */
    tle9879sys_init(&tle9879_sys,
    					CYBSP_D11,
                        CYBSP_D12,
                        CYBSP_D13,
                        NULL,
                        CYBSP_D4,
                        CYBSP_D5,
                        CYBSP_D6,
                        CYBSP_D7,
                        &numberOfBoards);
    if (tle9879_sys.board_count == 0)
    {
        printf("Motor board not detected. Exiting Motor task.\n");
        vTaskDelete(NULL);
    }

    tle9879sys_setMode(&tle9879_sys, FOC, 1, false);
    
    bool motorState=false;
    int currentPercentage=0;
    int desiredPercentage=0;

    while(1)
    {
    	rtos_api_result = xQueueReceive(motor_value_q, &desiredPercentage, RPM_CHANGE_INTERVAL);

       	/* Value has been received from the queue (i.e. not a timeout) */
        if(rtos_api_result == pdTRUE)
        {
            if(desiredPercentage < RPM_PERCENT_MIN) /* Any value less than 10% will result in stopping the motor */
                desiredPercentage = 0;
        
            if(desiredPercentage>100)
                desiredPercentage = 100;
        }

        if(currentPercentage != desiredPercentage)
        {
            if(abs(currentPercentage-desiredPercentage) < RPM_CHANGE_RATE)
                currentPercentage = desiredPercentage;

            if (currentPercentage < desiredPercentage)
                currentPercentage = currentPercentage + RPM_CHANGE_RATE;
            if (currentPercentage > desiredPercentage)
                currentPercentage = currentPercentage - RPM_CHANGE_RATE;

            if(currentPercentage>0 && motorState==false)
            {
                tle9879sys_setMotorMode(&tle9879_sys, START_MOTOR, 1);
                motorState = true;
            }
            if(currentPercentage == 0 && motorState==true)
            {
                tle9879sys_setMotorMode(&tle9879_sys, STOP_MOTOR, 1);
                motorState = false;
            }

            float motorSpeed = ((float)(currentPercentage-RPM_PERCENT_MIN))/(float)(100.0-RPM_PERCENT_MIN) * (RPM_MAX - RPM_MIN) + RPM_MIN;

            tle9879sys_setMotorSpeed(&tle9879_sys, motorSpeed, 1);
            printf("Current %d%% Desired=%d%% Speed=%f\n",currentPercentage,desiredPercentage,motorSpeed);
        }
    }
}

In order to simplify the CapSense and cloud tasks I provide a function to submit a new percent to the queue.

void motor_update(int speed)
{
    if(motor_value_q)
        xQueueOverwrite(motor_value_q,&speed);
}

6. Update the CapSense task to submit to the motor_task

Lets fix the CapSense task to submit to the motor queue.  First fix the include.

#include "motor_task.h"

Then update the buttons to use the submit function

if((0u != button0_status) && (0u == button0_status_prev))
{
    printf("Button 0 pressed\n");
    motor_update(0); // Stop the Motor
}

if((0u != button1_status) && (0u == button1_status_prev))
{
    printf("Button 1 pressed\n");
    motor_update(75); // Set the motor to 75%
}

Fix the slider to use the submit function as well.

if((0u != slider_touched) && (slider_pos_prev != slider_pos ))
{
    printf("Slider position %d\n",slider_pos);
    motor_update(slider_pos);
}

7. Fix the Cloud Task

Remember that we copied our cloud task from the remote control.  It publishes MQTT messages.  We need to fix the cloud task to subscribe instead of publish.   If you recall from Lesson 4 we submit the messages as JSON.  To decode the JSON I will use a JSON parser.

#include "cy_json_parser.h"

The function prototypes are almost the same, except for a subscribe instead of publish, plus a JSON callback

static void cloud_connectWifi();
static void cloud_startMQTT();
static void cloud_subscribeMQTT();
static void cloud_mqtt_event_cb( cy_mqtt_t mqtt_handle, cy_mqtt_event_t event, void *user_data);
static cy_rslt_t json_cb(cy_JSON_object_t *json_object, void *arg);

The cloud task is much simpler this time.  connect, start, subscribe and then wait for some action.

void cloud_task(void* param)
{
    (void)param;

    cloud_connectWifi();
    cloud_startMQTT();
    cloud_subscribeMQTT();

    for(;;)
    {
        vTaskSuspend(NULL);
    }
}

To subscribe you just setup a subscribe info structure.  Then call the subscribe.  You also need to configure the JSON parser.

static void cloud_subscribeMQTT()
{

    cy_rslt_t result;

    cy_mqtt_subscribe_info_t    sub_msg[1];

    /* Subscribe to motor speed MQTT messages */
    sub_msg[0].qos = 0;
    sub_msg[0].topic = CLOUD_MQTT_TOPIC;
    sub_msg[0].topic_len = strlen(sub_msg[0].topic);

    result = cy_mqtt_subscribe( mqtthandle, sub_msg, 1 );
    CY_ASSERT(result == CY_RSLT_SUCCESS);
    printf("Subscribe Success to Topic %s\n",CLOUD_MQTT_TOPIC);

    /* Register JSON callback function */
    cy_JSON_parser_register_callback(json_cb, NULL);

}

When you get a callback from the MQTT, you will look and see which topic.  If it matches the motor topic then call the JSON parser.

static void cloud_mqtt_event_cb( cy_mqtt_t mqtt_handle, cy_mqtt_event_t event, void *user_data )
{
    cy_mqtt_publish_info_t *received_msg;
    printf( "\nMQTT App callback with handle : %p \n", mqtt_handle );
    (void)user_data;
    switch( event.type )
    {
        case CY_MQTT_EVENT_TYPE_DISCONNECT :
            if( event.data.reason == CY_MQTT_DISCONN_TYPE_BROKER_DOWN )
            {
                printf( "\nCY_MQTT_DISCONN_TYPE_BROKER_DOWN .....\n" );
            }
            else
            {
                printf( "\nCY_MQTT_DISCONN_REASON_NETWORK_DISCONNECTION .....\n" );
            }
            break;
        case CY_MQTT_EVENT_TYPE_PUBLISH_RECEIVE :
            received_msg = &(event.data.pub_msg.received_message);
            printf( "Incoming Publish Topic Name: %.*s\n", received_msg->topic_len, received_msg->topic );
            printf( "Incoming Publish message Packet Id is %u.\n", event.data.pub_msg.packet_id );
            printf( "Incoming Publish Message : %.*s.\n\n", ( int )received_msg->payload_len, ( const char * )received_msg->payload );
            if(memcmp(received_msg->topic, CLOUD_MQTT_TOPIC, strlen(CLOUD_MQTT_TOPIC)) == 0) /* Topic matches the motor speed topic */
            {
                    cy_JSON_parser(received_msg->payload, received_msg->payload_len);
            }
            
            break;
        default :
            printf( "\nUNKNOWN EVENT .....\n" );
            break;
    }
}

If the JSON parser matches, you call the motor_update function with the new requested speed.

/* This is the callback from the cy_JSON_parser function. It is called whenever
 * the parser finds a JSON object. */
static cy_rslt_t json_cb(cy_JSON_object_t *json_object, void *arg)
{
    int motorSpeed;

    if(memcmp(json_object->object_string, MOTOR_KEY, json_object->object_string_length) == 0)
    {
        if(json_object->value_type == JSON_NUMBER_TYPE)
        {
            /* Add null termination to the value and then convert to a number */
            char resultString[json_object->value_length + 1];
            memcpy(resultString, json_object->value, json_object->value_length);
            resultString[json_object->value_length] = 0;
            motorSpeed = (uint8_t) atoi(resultString);
            printf("Received speed value from cloud: %d\n", motorSpeed);
            motor_update(motorSpeed);
        }
    }
    return CY_RSLT_SUCCESS;
}

8. Fix main.c to start the motor task

In main.c you need to “do the needful” starting with an include.

#include "motor_task.h"

Then starting the motor task.

xTaskCreate(motor_task,    "Motor"     ,configMINIMAL_STACK_SIZE*8  , NULL, 1, 0);

9. Test

Resources for Project

You can find the completed project in your project creator dialog by filtering for “IoT Expert Embedded”.  This is lesson6

You can also clone this project at git@github.com:iotexpert/ew21-lesson6.git or https://github.com/iotexpert/ew21-lesson6

Recommended Posts

No comment yet, add your voice below!


Add a Comment

Your email address will not be published. Required fields are marked *