Solve system of ODEs through the app layer

[NageshEranki]

Hi everyone!

I was toying around with the app layer and I implemented a Runge-Kutta (4th order) method to solve a system of ordinary differential equations. I chose a simple undamped harmonic oscillator for my system equations. Here are the plots generated through the client:

Here is the source code for your reference:

/**
 * app_foo.c: Demonstrate the workflow of using the app layer
 */

#include "app.h"
#include "app_channel.h"
#include "log.h"
#include "debug.h"

#include "FreeRTOS.h"
#include "task.h"

#define DEBUG_MODULE "HELLOWORLD"

// Loop counter
static uint64_t counter = 0;

static float xn1 = -1.0f;
static float xn2 = 0.0f;

// time step size
static float h = 0.1f;

static float k1 = 0.0f;
static float k2 = 0.0f;
static float k3 = 0.0f;
static float k4 = 0.0f;

static float l1 = 0.0f;
static float l2 = 0.0f;
static float l3 = 0.0f;
static float l4 = 0.0f;

// RHS 'f'
float f(float x1, float x2)
{
  return x2;
}

// RHS 'g'
float g(float x1, float x2)
{
  // return -2*x1+3*x2;
  return -2*x1;
}

void appMain()
{

  /*
      The 'main loop' that runs forever. This function should NEVER RETURN!

      This function iterates to solve systems of ODEs using the Runge-Kutta 4 method.

      x1dot = x2;
      x2dot = -2x1;
  */
  DEBUG_PRINT("Started appTask! ...\n");

  vTaskDelay(M2T(10000));

  // Iterate!
  while(1) {

    if (1)
    {
      k1 = h * f(xn1, xn2);
      l1 = h * g(xn1, xn2);
      k2 = h * f(xn1+k1/2.0f, xn2+l1/2.0f);
      l2 = h * g(xn1+k1/2.0f, xn2+l1/2.0f);
      k3 = h * f(xn1+k2/2.0f, xn2+l2/2.0f);
      l3 = h * g(xn1+k2/2.0f, xn2+l2/2.0f);
      k4 = h * f(xn1+k3, xn2+l3);
      l4 = h * g(xn1+k3, xn2+l3);

      xn1 = xn1 + 1.0f/6.0f * (k1 + 2*k2 + 2*k3 + k4);
      xn2 = xn2 + 1.0f/6.0f * (l1 + 2*l2 + 2*l3 + l4);
    }
    

    // IMPORTANT: Must include this artificial delay.
    // this loop must yield to the FreeRTOS scheduler
    // compulsorily.
    vTaskDelay(M2T(100));


    // Increment loop counter
    counter++;
  }
}

LOG_GROUP_START(rk4solve)
LOG_ADD(LOG_FLOAT, xn1, &xn1)
LOG_ADD(LOG_FLOAT, xn2, &xn2)
LOG_GROUP_STOP(rk4solve)

I had no further plans at all with this toy project. It was simply an exercise to explore the functionality of the Crazyflie! Let me know what you think!

[knmcguire]

Nice! See that the Crazyflie can do smart things :D this is University grade calculations !

Thanks for sharing!