rl_tools_controller.c

#define NEW_RL_TOOLS_CONTROLLER


#include "debug.h"
#include "usec_time.h"
#include <math.h>
#include "math3d.h"
#include "log.h"
#include "param.h"
#include "motors.h"
#include "watchdog.h"
#include "controller_pid.h"
#include "controller_mellinger.h"
#include "controller_indi.h"
#include "controller_brescianini.h"
#include "power_distribution.h"
#ifdef NEW_RL_TOOLS_CONTROLLER
#include <rl_tools/inference/applications/l2f/c_interface.h>
#else
#include "rl_tools_adapter.h"
#endif
#include "stabilizer_types.h"
#include "pm.h"
#include "task.h"


#include "benchmark.h"

#define CONTROL_INTERVAL_MS 2
#define CONTROL_INTERVAL_US (CONTROL_INTERVAL_MS * 1000)
#define CONTROL_PACKET_TIMEOUT_USEC (1000*200)
#define BEHIND_SCHEDULE_MESSAGE_MIN_INTERVAL (1000000)
#define CONTROL_INVOCATION_INTERVAL_ALPHA 0.95f
#define DEBUG_MEASURE_FORWARD_TIME
#define MIN_RPM 0
#define MAX_RPM 21702.1
#define WAYPOINT_NAVIGATION_NUMBER_OF_POINTS (5)
#define WARMUP_TIME (1000 * 500)
#define DWT_CYCCNT  (*((volatile uint32_t*)0xE0001004))
#define STATUS_MESSAGE_SIZE 256

char status_message[STATUS_MESSAGE_SIZE];

typedef enum ControllerState{
  STATE_RESET,
  STATE_FORWARD,
  STATE_FINISHED 
} ControllerState;

// State
static ControllerState controller_state;

// Counters
static uint64_t controller_tick = 0; // Number of control function invocations
static uint64_t forward_tick = 0; // Number of forward passes
static uint64_t rlt_policy_tick = 0; // Number of forward passes

// Timestamps
static uint64_t timestamp_last_reset;
static uint64_t timestamp_last_behind_schedule_message = 0;
static uint64_t timestamp_last_control_invocation = 0;
static uint64_t timestamp_last_control_packet_received = 0;
static uint64_t timestamp_last_control_packet_received_hover = 0;
static uint64_t timestamp_controller_activation;
static uint64_t timestamp_pre_set_motors;

// Logging variables
static float control_invocation_interval = 0;

// Control variables: input
static float target_pos[3] = {0, 0, 0};
static float target_vel[3] = {0, 0, 0};
static float pos_error[3] = {0, 0, 0};
static float relative_pos[3] = {0, 0, 0};
static float origin[3] = {0, 0, 0};

static float waypoint_navigation_target_vel = 0.0;

static float pos_distance_limit_position;
static float vel_distance_limit_position;
static float pos_distance_limit_figure_eight;
static float vel_distance_limit_figure_eight;
static float pos_distance_limit_mellinger;
static float vel_distance_limit_mellinger;
static float pos_distance_limit_bresciani;
static float vel_distance_limit_bresciani;
static uint8_t mellinger_enable_integrators;
static uint8_t log_set_motors = 0;
static float velocity_cmd_multiplier, velocity_cmd_p_term;
#ifdef NEW_RL_TOOLS_CONTROLLER
static RLtoolsInferenceExecutorStatus non_healthy_status_intermediate, non_healthy_status_native;
#endif
uint32_t healthy_status_count_intermediate, non_healthy_status_count_intermediate, healthy_status_count_native, non_healthy_status_count_native;

enum Mode{
  NORMAL = 0, // take setpoints from the standard cf firmware pipeline
  POSITION = 1, // hardcoded position (e.g. z offset)
  WAYPOINT_NAVIGATION = 2,
  WAYPOINT_NAVIGATION_DYNAMIC = 3,
  FIGURE_EIGHT = 4
};
enum TriggerMode{
  RL_TOOLS_PACKET = 0,
  HOVER_PACKET = 1,
};
static uint8_t mode;
static uint8_t trigger_mode;
static uint8_t learned_controller_packet_gates_motors;
static float trajectory[WAYPOINT_NAVIGATION_NUMBER_OF_POINTS][3] = {
  {0.0, 0.0, 0.0},
  {1.0, 0.0, 0.0},
  {1.0, 1.0, 0.0},
  {0.0, 1.0, 0.0},
  {0.0, 0.0, 0.0},
};
static float target_height;

static uint64_t waypoint_navigation_timestamp_start;
static uint8_t  waypoint_navigation_dynamic_current_waypoint = 0;
static float    waypoint_navigation_dynamic_threshold = 0.1;
static float    waypoint_navigation_point_duration = 4;
static float    waypoint_navigation_trajectory_scale = 0.5;

static float    figure_eight_interval = 5.5;
static float    figure_eight_warmup_time; 
static float    figure_eight_scale = 1.0;
static float    figure_eight_progress = 0;
static uint64_t figure_eight_last_invocation;
static float    target_height_figure_eight;

static float state_input[13];
static float action_output[4];

const uint8_t motors[4] = {MOTOR_M1, MOTOR_M2, MOTOR_M3, MOTOR_M4};
static uint8_t set_motors_overwrite = 0;
static uint16_t motor_cmd[4];
static float motor_cmd_float[4];
static float motor_cmd_divider, motor_cmd_divider_warmup;
static bool prev_set_motors, prev_pre_set_motors;
static uint8_t use_pre_set_warmup;

static motors_thrust_uncapped_t motorThrustUncapped;
static motors_thrust_uncapped_t motorThrustBatCompUncapped;
static motors_thrust_pwm_t motorPwm;

static setpoint_t last_setpoint;

static uint8_t hand_test = 0; // 0 = off; 1 = setpoint; 2 = angular velocity rejection; 3 = angular velocity rejection + orientation rejection;
static uint8_t use_orig_controller = 0;


static inline float clip(float v, float low, float high){
  if(v < low){
    return low;
  }
  else{
    if(v > high){
      return high;
    }
    else{
      return v;
    }
  }
}

static inline void update_state(const sensorData_t* sensors, const state_t* state){
  if(hand_test == 0){
    float POS_DISTANCE_LIMIT = mode == FIGURE_EIGHT ? pos_distance_limit_figure_eight : pos_distance_limit_position;
    state_input[ 0] = clip(state->position.x - target_pos[0], -POS_DISTANCE_LIMIT, POS_DISTANCE_LIMIT);
    state_input[ 1] = clip(state->position.y - target_pos[1], -POS_DISTANCE_LIMIT, POS_DISTANCE_LIMIT);
    state_input[ 2] = clip(state->position.z - target_pos[2], -POS_DISTANCE_LIMIT, POS_DISTANCE_LIMIT);
  }
  else{
    state_input[ 0] = 0;
    state_input[ 1] = 0;
    state_input[ 2] = 0;
  }
  if(hand_test == 0 || hand_test == 3){
    state_input[ 3] = state->attitudeQuaternion.w;
    state_input[ 4] = state->attitudeQuaternion.x;
    state_input[ 5] = state->attitudeQuaternion.y;
    state_input[ 6] = state->attitudeQuaternion.z;
  }
  else{
    state_input[ 3] = 1;
    state_input[ 4] = 0;
    state_input[ 5] = 0;
    state_input[ 6] = 0;
  }
  if(hand_test == 0){
    float VEL_DISTANCE_LIMIT = mode == FIGURE_EIGHT ? vel_distance_limit_figure_eight : vel_distance_limit_position;
    state_input[ 7] = clip(state->velocity.x - target_vel[0], -VEL_DISTANCE_LIMIT, VEL_DISTANCE_LIMIT);
    state_input[ 8] = clip(state->velocity.y - target_vel[1], -VEL_DISTANCE_LIMIT, VEL_DISTANCE_LIMIT);
    state_input[ 9] = clip(state->velocity.z - target_vel[2], -VEL_DISTANCE_LIMIT, VEL_DISTANCE_LIMIT);
  }
  else{
    state_input[ 7] = 0;
    state_input[ 8] = 0;
    state_input[ 9] = 0;
  }
  if(hand_test != 1){
    state_input[10] = radians(sensors->gyro.x);
    state_input[11] = radians(sensors->gyro.y);
    state_input[12] = radians(sensors->gyro.z);
  }
  else{
    state_input[10] = 0;
    state_input[11] = 0;
    state_input[12] = 0;
  }
}

void rl_tools_controller_packet_received(){
  uint64_t now = usecTimestamp();
  timestamp_last_control_packet_received = now;
}
// void rl_tools_controller_hover_packet_received(){
//   uint64_t now = usecTimestamp();
//   timestamp_last_control_packet_received_hover = now;
//   DEBUG_PRINT("Hover packet received\n");
// }


void controllerOutOfTreeInit(void){

  controller_state = STATE_RESET;
  controller_tick = 0;
  rlt_policy_tick = 0;
  motor_cmd_divider = 1.0;
  motor_cmd_divider_warmup = 4.0;

  motor_cmd[0] = 0;
  motor_cmd[1] = 0;
  motor_cmd[2] = 0;
  motor_cmd[3] = 0;
  motor_cmd_float[0] = 0;
  motor_cmd_float[1] = 0;
  motor_cmd_float[2] = 0;
  motor_cmd_float[3] = 0;
  timestamp_last_reset = usecTimestamp();
  prev_set_motors = false;
  prev_pre_set_motors = false;
  use_pre_set_warmup = 1;
  timestamp_last_control_packet_received = 0;
  timestamp_last_control_packet_received_hover = 0;
  timestamp_last_behind_schedule_message = 0;
  control_invocation_interval = 0;
  forward_tick = 0;
  hand_test = 0;
  waypoint_navigation_timestamp_start = 0;
  waypoint_navigation_trajectory_scale = 0.5;
  relative_pos[0] = 0;
  relative_pos[1] = 0;
  relative_pos[2] = 0;
  log_set_motors = 0;
  
  waypoint_navigation_target_vel = 0.0;

  pos_distance_limit_position = 0.5f;
  vel_distance_limit_position = 2.0f;
  pos_distance_limit_figure_eight = 0.6f;
  vel_distance_limit_figure_eight = 2.0f;
  pos_distance_limit_mellinger = 0.2f;
  vel_distance_limit_mellinger = 1.0f;
  pos_distance_limit_bresciani = 0.2f;
  vel_distance_limit_bresciani = 1.0f;
  mellinger_enable_integrators = 1;
  velocity_cmd_multiplier = 1;
  velocity_cmd_p_term = 0.0;

  action_output[0] = 0;
  action_output[1] = 0;
  action_output[2] = 0;
  action_output[3] = 0;

  target_height = 0.3;
  target_height_figure_eight = 0.0;

  mode = NORMAL;
  // mode = POSITION;
  // mode = FIGURE_EIGHT;
  trigger_mode = RL_TOOLS_PACKET;
  // trigger_mode = HOVER_PACKET;
  learned_controller_packet_gates_motors = true;
  use_orig_controller = 0;
  waypoint_navigation_dynamic_current_waypoint = 0;
  // waypoint_navigation_dynamic_threshold = 0;

  figure_eight_interval = 5.5;
  figure_eight_scale = 1;
  figure_eight_progress = 0;
  figure_eight_warmup_time = 2;
  healthy_status_count_intermediate = 0;
  non_healthy_status_count_intermediate = 0;
  healthy_status_count_native = 0;
  non_healthy_status_count_native = 0;

  controllerPidInit();
  controllerMellingerFirmwareInit();
  controllerINDIInit();
  controllerBrescianiniInit();
  rl_tools_inference_applications_l2f_init();

  DEBUG_PRINT("Checkpoint: %s\n", rl_tools_inference_applications_l2f_checkpoint_name());
}

bool controllerOutOfTreeTest(void){
  RLtoolsInferenceApplicationsL2FAction test_output;
  float test_absdiff = rl_tools_inference_applications_l2f_test(&test_output);
  if(test_absdiff < 0){
    test_absdiff = -test_absdiff;
  }
  DEBUG_PRINT("RLtools controller test, abs diff: %f\n", test_absdiff);
  for(int i = 0; i < RL_TOOLS_INTERFACE_APPLICATIONS_L2F_ACTION_DIM; i++){
    DEBUG_PRINT("RLtools controller: Test action %d: %f\n", i, test_output.action[i]);
  }
  if(test_absdiff > 0.2){
    return false;
  }
  return controllerPidTest() && controllerMellingerFirmwareTest() && controllerINDITest() && controllerBrescianiniTest();
}

static void batteryCompensation(const motors_thrust_uncapped_t* motorThrustUncapped, motors_thrust_uncapped_t* motorThrustBatCompUncapped)
{
  float supplyVoltage = pmGetBatteryVoltage();

  for (int motor = 0; motor < STABILIZER_NR_OF_MOTORS; motor++)
  {
    motorThrustBatCompUncapped->list[motor] = motorsCompensateBatteryVoltage(motor, motorThrustUncapped->list[motor], supplyVoltage);
  }
}
static void setMotorRatios(const motors_thrust_pwm_t* motorPwm)
{
  motorsSetRatio(MOTOR_M1, motorPwm->motors.m1);
  motorsSetRatio(MOTOR_M2, motorPwm->motors.m2);
  motorsSetRatio(MOTOR_M3, motorPwm->motors.m3);
  motorsSetRatio(MOTOR_M4, motorPwm->motors.m4);
}


static inline void every_500ms(){
#ifdef PRINT_TWIST
  // DEBUG_PRINT("tw.l: %5.2f, %5.2f, %5.2f tw.a: %5.2f, %5.2f, %5.2f\n", state_input[7], state_input[8], state_input[9], state_input[10], state_input[11], state_input[12]);
  // DEBUG_PRINT("q: %5.2f, %5.2f, %5.2f, %5.2f\n", state_input[3], state_input[4], state_input[5], state_input[6]);
#endif
}

static inline void every_1000ms(){
#ifdef PRINT_RPY
  // DEBUG_PRINT("rpy: %5.2f, %5.2f, %5.2f\n", attitude_rpy[0], attitude_rpy[1], attitude_rpy[2]);
#endif
  // DEBUG_PRINT("Heap: %u bytes free\n", xPortGetFreeHeapSize());

  // DEBUG_PRINT("Last setpoint: x disposition/mode %f/%f/%d\n", last_setpoint.position.x, last_setpoint.velocity.x, last_setpoint.mode.x);
  // DEBUG_PRINT("Last setpoint: y disposition/mode %f/%f/%d\n", last_setpoint.position.y, last_setpoint.velocity.y, last_setpoint.mode.y);
  // DEBUG_PRINT("Last setpoint: z disposition/mode %f/%f/%d\n", last_setpoint.position.z, last_setpoint.velocity.z, last_setpoint.mode.z);
}

static inline void every_10000ms(){
  // DEBUG_PRINT("control invocation interval %f\n", (double)control_invocation_interval);
}

static inline void trigger_every(uint64_t controller_tick){
  if(controller_tick > 3000){
    if(controller_tick % 500 == 0){
      every_500ms();
    }
    if(controller_tick  % 1000 == 150){
      every_1000ms();
    }
    if(controller_tick % 10000 == 9300){
      every_10000ms();
    }
  }
}

static void print_mode(stab_mode_t mode){
  switch(mode){
    case modeDisable:
      DEBUG_PRINT("modeDisable\n");
      break;
    case modeAbs:
      DEBUG_PRINT("modeAbs\n");
      break;
    case modeVelocity:
      DEBUG_PRINT("modeVelocity\n");
      break;
  }
}

void debugging_pool_print(){
#ifdef RL_TOOLS_ENABLE_DEBUGGING_POOL
  DEBUG_PRINT("RLtools controller pool size %d\n", rl_tools_debugging_pool_index);
  for(unsigned pool_i=0; pool_i < rl_tools_debugging_pool_index; pool_i++){
    DEBUG_PRINT("Debug pool %d %s: ", pool_i, rl_tools_debugging_pool_names[pool_i]);
    for(unsigned pool_step_i=0; pool_step_i < rl_tools_debugging_pool_indices[pool_i]; pool_step_i++){
      DEBUG_PRINT("%f ", rl_tools_debugging_pool[pool_i][pool_step_i]);
    }
    DEBUG_PRINT("\n");
  }
#endif
}

void controllerOutOfTree(control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick) {

  uint64_t now = usecTimestamp();
  if(setpoint->mode.x == modeVelocity && setpoint->mode.y == modeVelocity){
    timestamp_last_control_packet_received_hover = now;
  }

  last_setpoint = *setpoint;
  watchdogReset();
  control_invocation_interval *= CONTROL_INVOCATION_INTERVAL_ALPHA;
  control_invocation_interval += (1-CONTROL_INVOCATION_INTERVAL_ALPHA) * (now - timestamp_last_control_invocation);
  timestamp_last_control_invocation = now;
  uint64_t relevant_timestamp_last_control_packet_received = trigger_mode == RL_TOOLS_PACKET ? timestamp_last_control_packet_received : timestamp_last_control_packet_received_hover;
  bool pre_set_motors = (now - relevant_timestamp_last_control_packet_received < CONTROL_PACKET_TIMEOUT_USEC)  || (set_motors_overwrite == 1 && motor_cmd_divider >= 3);
  bool set_motors = false;

  if(!prev_pre_set_motors && pre_set_motors){
    timestamp_pre_set_motors = now;
  }
  set_motors = pre_set_motors && (((now - timestamp_pre_set_motors) > WARMUP_TIME) || use_pre_set_warmup == 0);

  log_set_motors = set_motors ? 1 : 0;
  // set_rl_tools_overwrite_stabilizer(set_motors);
  if(!prev_set_motors && set_motors){
    waypoint_navigation_timestamp_start = now;
    timestamp_controller_activation = now;
    waypoint_navigation_dynamic_current_waypoint = 0;
    origin[0] = state->position.x;
    origin[1] = state->position.y;
    origin[2] = state->position.z + (mode == FIGURE_EIGHT ? target_height_figure_eight : target_height);
    figure_eight_last_invocation = now;
    figure_eight_progress = 0;
    controllerMellingerFirmwareInit();
    controllerINDIInit();
    // controllerMellingerFirmwareEnableIntegrators(MELLINGER_ENABLE_INTEGRATORS == 1);
    rl_tools_inference_applications_l2f_reset();
    DEBUG_PRINT("Controller activated\n");
    switch(mode){
      case NORMAL:
        DEBUG_PRINT("NORMAL mode \n");
        DEBUG_PRINT("\t x mode: "); print_mode(setpoint->mode.x);
        DEBUG_PRINT("\t y mode: "); print_mode(setpoint->mode.y);
        DEBUG_PRINT("\t z mode: "); print_mode(setpoint->mode.z);
        break;
      case POSITION:
        DEBUG_PRINT("POSITION mode\n");
        break;
      case WAYPOINT_NAVIGATION:
        DEBUG_PRINT("WAYPOINT_NAVIGATION mode\n");
        break;
      case WAYPOINT_NAVIGATION_DYNAMIC:
        DEBUG_PRINT("WAYPOINT_NAVIGATION_DYNAMIC mode\n");
        break;
      case FIGURE_EIGHT:
        DEBUG_PRINT("FIGURE_EIGHT mode\n");
        break;
    }
  }
  if(prev_set_motors && !set_motors){
    DEBUG_PRINT("Controller deactivated\n");
    for(uint8_t i=0; i<4; i++){
      motorsSetRatio(motors[i], 0);
    }
  }
  relative_pos[0] = state->position.x - origin[0];
  relative_pos[1] = state->position.y - origin[1];
  relative_pos[2] = state->position.z - origin[2];
  target_vel[0] = 0;
  target_vel[1] = 0;
  target_vel[2] = 0;
  switch(mode){
    case NORMAL:
      switch(setpoint->mode.x){
        case modeAbs:
        target_pos[0] = setpoint->position.x;
        target_vel[0] = setpoint->velocity.x;
        break;
        case modeVelocity:
        target_pos[0] = state->position.x - setpoint->velocity.x * velocity_cmd_p_term;
        target_vel[0] = setpoint->velocity.x * velocity_cmd_multiplier;
        break;
        case modeDisable:
        target_pos[0] = origin[0];
        target_vel[0] = 0;
        break;
      }
      switch(setpoint->mode.y){
        case modeAbs:
        target_pos[1] = setpoint->position.y;
        target_vel[1] = setpoint->velocity.y;
        break;
        case modeVelocity:
        target_pos[1] = state->position.y - setpoint->velocity.y * velocity_cmd_p_term;
        target_vel[1] = setpoint->velocity.y * velocity_cmd_multiplier;
        break;
        case modeDisable:
        target_pos[1] = origin[1];
        target_vel[1] = 0;
        break;
      }
      switch(setpoint->mode.z){
        case modeAbs:
        target_pos[2] = setpoint->position.z;
        target_vel[2] = setpoint->velocity.z;
        break;
        case modeVelocity:
        target_pos[2] = state->position.z - setpoint->velocity.z * velocity_cmd_p_term;
        target_vel[2] = setpoint->velocity.z * velocity_cmd_multiplier;
        break;
        case modeDisable:
        target_pos[2] = origin[2];
        target_vel[2] = 0;
        break;
      }
    break;
    case POSITION:
      target_pos[0] = origin[0];
      target_pos[1] = origin[1];
      target_pos[2] = origin[2];
      break;
    case WAYPOINT_NAVIGATION:
    {
      uint64_t elapsed_since_start = (now-waypoint_navigation_timestamp_start);
      int current_point = (elapsed_since_start / ((int)(waypoint_navigation_point_duration * 1000 * 1000))) % WAYPOINT_NAVIGATION_NUMBER_OF_POINTS;
      target_pos[0] = trajectory[current_point][0] * waypoint_navigation_trajectory_scale + origin[0];
      target_pos[1] = trajectory[current_point][1] * waypoint_navigation_trajectory_scale + origin[1];
      target_pos[2] = trajectory[current_point][2] * waypoint_navigation_trajectory_scale + origin[2];
    }
      break;
    case WAYPOINT_NAVIGATION_DYNAMIC:
      {
        float x = trajectory[waypoint_navigation_dynamic_current_waypoint][0] - relative_pos[0];
        float y = trajectory[waypoint_navigation_dynamic_current_waypoint][1] - relative_pos[1];
        float z = trajectory[waypoint_navigation_dynamic_current_waypoint][2] - relative_pos[2];

        float current_dist = sqrtf(x*x + y*y + z*z);
        if(current_dist < waypoint_navigation_dynamic_threshold){
          waypoint_navigation_dynamic_current_waypoint = (waypoint_navigation_dynamic_current_waypoint + 1) % WAYPOINT_NAVIGATION_NUMBER_OF_POINTS;
          DEBUG_PRINT("Next waypoint %d, [%f, %f, %f]\n", waypoint_navigation_dynamic_current_waypoint, trajectory[waypoint_navigation_dynamic_current_waypoint][0], trajectory[waypoint_navigation_dynamic_current_waypoint][1], trajectory[waypoint_navigation_dynamic_current_waypoint][2]);
        }
        target_pos[0] = origin[0] + trajectory[waypoint_navigation_dynamic_current_waypoint][0];
        target_pos[1] = origin[1] + trajectory[waypoint_navigation_dynamic_current_waypoint][1];
        target_pos[2] = origin[2] + trajectory[waypoint_navigation_dynamic_current_waypoint][2];
        target_vel[0] = x / current_dist * waypoint_navigation_target_vel;
        target_vel[1] = y / current_dist * waypoint_navigation_target_vel;
        target_vel[2] = z / current_dist * waypoint_navigation_target_vel;
      }
      break;
    case FIGURE_EIGHT:
      {
        float t = (now - timestamp_controller_activation) / 1000000.0f;
        float dt = (now - figure_eight_last_invocation) / 1000000.0f;
        float target_speed = 1/figure_eight_interval;
        float speed = target_speed;
        if(t < figure_eight_warmup_time){
          speed = target_speed * t/figure_eight_warmup_time;
        }
        figure_eight_progress += dt * speed;
        float progress = figure_eight_progress;
        target_pos[0] = origin[0] + cosf(progress*2*M_PI + M_PI / 2) * figure_eight_scale;
        target_vel[0] = -sinf(progress*2*M_PI + M_PI / 2) * figure_eight_scale * 2 * M_PI * speed;
        target_pos[1] = origin[1] + sinf(2*(progress*2*M_PI + M_PI / 2)) / 2.0f * figure_eight_scale;
        target_vel[1] = cosf(2*(progress*2*M_PI + M_PI / 2)) / 2.0f * figure_eight_scale * 4 * M_PI * speed;
        target_pos[2] = origin[2];
        figure_eight_last_invocation = now;
      }
      break;
  }
  pos_error[0] = target_pos[0] - state->position.x;
  pos_error[1] = target_pos[1] - state->position.y;
  pos_error[2] = target_pos[2] - state->position.z;

  trigger_every(controller_tick);
  prev_set_motors = set_motors;
  prev_pre_set_motors = pre_set_motors;

  update_state(sensors, state);
  {
    int64_t before = usecTimestamp();
    uint32_t start_cycle = DWT->CYCCNT;
#ifdef NEW_RL_TOOLS_CONTROLLER
    RLtoolsInferenceApplicationsL2FObservation observation;
    for(uint8_t i=0; i<4; i++){
      if(i < 3){
        observation.position[i] = state_input[i];
        observation.orientation[i] = state_input[3+i];
        observation.linear_velocity[i] = state_input[3+4+i];
        observation.angular_velocity[i] = state_input[3+4+3+i];
        observation.previous_action[i] = action_output[i];
      }
      else{
        observation.orientation[i] = state_input[3+i];
        observation.previous_action[i] = action_output[i];
      }
    }
    RLtoolsInferenceApplicationsL2FAction action;
    RLtoolsInferenceExecutorStatus rlt_status;
    rlt_status = rl_tools_inference_applications_l2f_control(before * 1000, &observation, &action);
    if(!rlt_status.OK){
      if(rlt_status.source == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL){
        if(rlt_status.step_type == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_INTERMEDIATE){
          non_healthy_status_intermediate = rlt_status;
          non_healthy_status_count_intermediate++;
        }
        else{
          non_healthy_status_native = rlt_status;
          non_healthy_status_count_native++;
        }
      }
    }
    else{
      if(rlt_status.source == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL){
        if(rlt_status.step_type == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_INTERMEDIATE){
          healthy_status_count_intermediate++;
        }
        else{
          healthy_status_count_native++;
        }
      }
    }
    for(uint8_t i=0; i<4; i++){
      action_output[i] = action.action[i];
    }
    if(rlt_status.source == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL){
      rlt_policy_tick++;
    }
#else
    rl_tools_control(state_input, action_output);
#endif
    uint32_t end_cycle = DWT->CYCCNT;
    uint32_t cycles = end_cycle - start_cycle;
    int64_t after = usecTimestamp();
    if ((rlt_status.source == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL) && rlt_policy_tick % 500 == 0){
      // DEBUG_PRINT("rl_tools_control took %lu cycles (%lldus)\n", cycles, after - before);
    }
    if((tick % (CONTROL_INTERVAL_MS * 1000) == 0)){
      #ifdef NEW_RL_TOOLS_CONTROLLER
      if(non_healthy_status_count_intermediate > 0){
        rl_tools_inference_executor_status_message(non_healthy_status_intermediate, status_message, STATUS_MESSAGE_SIZE);
        DEBUG_PRINT("%d / %d healthy intermediate statii, latest: %s\n", healthy_status_count_intermediate, (healthy_status_count_intermediate + non_healthy_status_count_intermediate), status_message);
      }
      else{
        // DEBUG_PRINT("%d healthy intermediate statii\n", healthy_status_count_intermediate);
      }
      non_healthy_status_count_intermediate = 0;
      healthy_status_count_intermediate = 0;
      if(non_healthy_status_count_native > 0){
        rl_tools_inference_executor_status_message(non_healthy_status_native, status_message, STATUS_MESSAGE_SIZE);
        DEBUG_PRINT("%d / %d healthy native statii, latest: %s\n", healthy_status_count_native, (healthy_status_count_native + non_healthy_status_count_native), status_message);
      }
      else{
        // DEBUG_PRINT("%d healthy native statii\n", healthy_status_count_native);
      }
      non_healthy_status_count_native = 0;
      healthy_status_count_native = 0;
      rl_tools_inference_executor_status_message(rlt_status, status_message, STATUS_MESSAGE_SIZE);
      // DEBUG_PRINT("RLtools controller status %s\n", status_message);
      #endif
      if(controller_tick > 1000){
        #ifdef RL_TOOLS_ENABLE_DEBUGGING_POOL
        debugging_pool_print();
        #endif
      }
    }
    if(use_orig_controller != 0){
      action_output[0] = -0.8;
      action_output[1] = -0.8;
      action_output[2] = -0.8;
      action_output[3] = -0.8;
    }
  }
  for(uint8_t i=0; i<4; i++){
    if (tick % (CONTROL_INTERVAL_MS * 1000) == 0){
      // DEBUG_PRINT("action_output[%d]: %f\n", i, action_output[i]);
    }
    float a_pp = (action_output[i] + 1)/2;
    float des_rpm = (MAX_RPM - MIN_RPM) * a_pp + MIN_RPM;
    float des_percentage = des_rpm / MAX_RPM;
    motor_cmd_float[i] = des_percentage;
    motor_cmd[i] = des_percentage * UINT16_MAX;
    if(set_motors && use_orig_controller == 0){
      motorsSetRatio(motors[i], clip((float)motor_cmd[i] / motor_cmd_divider, 0, UINT16_MAX));
    }
  }
  int64_t spare_time = CONTROL_INTERVAL_US - (now - timestamp_last_reset) ;
  if(spare_time < 0 && (now - timestamp_last_behind_schedule_message > BEHIND_SCHEDULE_MESSAGE_MIN_INTERVAL)){
    // DEBUG_PRINT("Learned Controller is behind schedule: %lldus/%dus\n", (int64_t)(now-timestamp_last_reset), CONTROL_INTERVAL_US);
    timestamp_last_behind_schedule_message = now;
  }
  timestamp_last_reset = usecTimestamp();
  if(!set_motors){
    if(pre_set_motors){
      for(uint8_t i=0; i<4; i++){
        motorsSetRatio(motors[i], UINT16_MAX / motor_cmd_divider_warmup);
      }
    }
    else{
      if(learned_controller_packet_gates_motors){
        // if the learned controller packet gates the motors, they are shut off. Otherwise we execute the normal controller stack (e.g. to handover in mid-air)
        motorsSetRatio(motors[0], 0);
        motorsSetRatio(motors[1], 0);
        motorsSetRatio(motors[2], 0);
        motorsSetRatio(motors[3], 0);
        // The stabilizer sends DShot after this controller returns; motorsStop()
        // would start a second transfer in the same control cycle.
      }
      else{
        controllerPid(control, setpoint, sensors, state, tick);
        powerDistribution(control, &motorThrustUncapped);
        batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped);
        powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm);
        setMotorRatios(&motorPwm);
        motor_cmd_float[0] = motorPwm.motors.m1/(float)UINT16_MAX;
        motor_cmd_float[1] = motorPwm.motors.m2/(float)UINT16_MAX;
        motor_cmd_float[2] = motorPwm.motors.m3/(float)UINT16_MAX;
        motor_cmd_float[3] = motorPwm.motors.m4/(float)UINT16_MAX;
      }
    }
  }
  else{
    if(use_orig_controller >= 1){
      setpoint->mode.x = modeAbs;
      setpoint->mode.y = modeAbs;
      setpoint->mode.z = modeAbs;
      setpoint->mode.yaw = modeAbs;
      setpoint->mode.pitch = modeDisable;
      setpoint->mode.roll = modeDisable;
      setpoint->mode.quat = modeDisable;
      setpoint->position.x = target_pos[0];
      setpoint->position.y = target_pos[1];
      setpoint->position.z = target_pos[2];
      setpoint->velocity.x = target_vel[0];
      setpoint->velocity.y = target_vel[1];
      setpoint->velocity.z = target_vel[2];
      setpoint->acceleration.x = 0;
      setpoint->acceleration.y = 0;
      setpoint->acceleration.z = 0;

      setpoint->attitude.yaw = 0;
      setpoint->attitude.pitch = 0;
      setpoint->attitude.roll = 0;
      setpoint->attitudeQuaternion.w = 1;
      setpoint->attitudeQuaternion.x = 0;
      setpoint->attitudeQuaternion.y = 0;
      setpoint->attitudeQuaternion.z = 0;
      setpoint->attitudeRate.yaw = 0;
      setpoint->attitudeRate.pitch = 0;
      setpoint->attitudeRate.roll = 0;

      setpoint->timestamp = xTaskGetTickCount();
      if(use_orig_controller == 1){
        controllerPid(control, setpoint, sensors, state, tick);
      }
      else{
        if(use_orig_controller == 2){
          setpoint->position.x = state->position.x + clip(target_pos[0] - state->position.x, -pos_distance_limit_mellinger, pos_distance_limit_mellinger);
          setpoint->position.y = state->position.y + clip(target_pos[1] - state->position.y, -pos_distance_limit_mellinger, pos_distance_limit_mellinger);
          setpoint->position.z = state->position.z + clip(target_pos[2] - state->position.z, -pos_distance_limit_mellinger, pos_distance_limit_mellinger);
          setpoint->velocity.x = state->velocity.x + clip(target_vel[0] - state->velocity.x, -vel_distance_limit_mellinger, vel_distance_limit_mellinger);
          setpoint->velocity.y = state->velocity.y + clip(target_vel[1] - state->velocity.y, -vel_distance_limit_mellinger, vel_distance_limit_mellinger);
          setpoint->velocity.z = state->velocity.z + clip(target_vel[2] - state->velocity.z, -vel_distance_limit_mellinger, vel_distance_limit_mellinger);
          controllerMellingerFirmware(control, setpoint, sensors, state, tick);
        }
        else{
          if(use_orig_controller == 3){
            controllerINDI(control, setpoint, sensors, state, tick);
          }
          else{
            setpoint->position.x = state->position.x + clip(target_pos[0] - state->position.x, -pos_distance_limit_bresciani, pos_distance_limit_bresciani);
            setpoint->position.y = state->position.y + clip(target_pos[1] - state->position.y, -pos_distance_limit_bresciani, pos_distance_limit_bresciani);
            setpoint->position.z = state->position.z + clip(target_pos[2] - state->position.z, -pos_distance_limit_bresciani, pos_distance_limit_bresciani);
            setpoint->velocity.x = state->velocity.x + clip(target_vel[0] - state->velocity.x, -vel_distance_limit_bresciani, vel_distance_limit_bresciani);
            setpoint->velocity.y = state->velocity.y + clip(target_vel[1] - state->velocity.y, -vel_distance_limit_bresciani, vel_distance_limit_bresciani);
            setpoint->velocity.z = state->velocity.z + clip(target_vel[2] - state->velocity.z, -vel_distance_limit_bresciani, vel_distance_limit_bresciani);
            controllerBrescianini(control, setpoint, sensors, state, tick);
          }
        }
      }
      powerDistribution(control, &motorThrustUncapped);
      batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped);
      powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm);
      setMotorRatios(&motorPwm);
    }
  }
  controller_tick++;
}



PARAM_GROUP_START(rlt)
PARAM_ADD(PARAM_UINT8, trigger, &trigger_mode)
PARAM_ADD(PARAM_UINT8, gate, &learned_controller_packet_gates_motors)
PARAM_ADD(PARAM_UINT8, motor_warmup, &use_pre_set_warmup)
PARAM_ADD(PARAM_FLOAT, motor_div, &motor_cmd_divider)
PARAM_ADD(PARAM_FLOAT, motor_div_wu, &motor_cmd_divider_warmup)
PARAM_ADD(PARAM_FLOAT, target_z, &target_height)
PARAM_ADD(PARAM_FLOAT, target_z_fe, &target_height_figure_eight)
PARAM_ADD(PARAM_UINT8, smo, &set_motors_overwrite)
PARAM_ADD(PARAM_UINT8, ht, &hand_test)
PARAM_ADD(PARAM_UINT8, wn, &mode)
PARAM_ADD(PARAM_FLOAT, ts, &waypoint_navigation_trajectory_scale)
PARAM_ADD(PARAM_FLOAT, wpt, &waypoint_navigation_dynamic_threshold)
PARAM_ADD(PARAM_FLOAT, wntv, &waypoint_navigation_target_vel)
PARAM_ADD(PARAM_FLOAT, wni, &waypoint_navigation_point_duration)
PARAM_ADD(PARAM_FLOAT, fewt, &figure_eight_warmup_time)
PARAM_ADD(PARAM_FLOAT, fei,  &figure_eight_interval)
PARAM_ADD(PARAM_FLOAT, fes,  &figure_eight_scale)
PARAM_ADD(PARAM_FLOAT, pdlp,  &pos_distance_limit_position)
PARAM_ADD(PARAM_FLOAT, pdlfe, &pos_distance_limit_figure_eight)
PARAM_ADD(PARAM_FLOAT, vdlp,  &vel_distance_limit_position)
PARAM_ADD(PARAM_FLOAT, vdlfe, &vel_distance_limit_figure_eight)
PARAM_ADD(PARAM_FLOAT, pdlm,  &pos_distance_limit_mellinger)
PARAM_ADD(PARAM_FLOAT, vdlm,  &vel_distance_limit_mellinger)
PARAM_ADD(PARAM_UINT8, orig, &use_orig_controller)
PARAM_ADD(PARAM_UINT8, mei, &mellinger_enable_integrators)
PARAM_ADD(PARAM_FLOAT, vcmdm, &velocity_cmd_multiplier)
PARAM_ADD(PARAM_FLOAT, vcmdp, &velocity_cmd_p_term)
PARAM_GROUP_STOP(rlt)


LOG_GROUP_START(rltm)
LOG_ADD(LOG_FLOAT, m1, &motor_cmd_float[0])
LOG_ADD(LOG_FLOAT, m2, &motor_cmd_float[1])
LOG_ADD(LOG_FLOAT, m3, &motor_cmd_float[2])
LOG_ADD(LOG_FLOAT, m4, &motor_cmd_float[3])
LOG_GROUP_STOP(rltm)

LOG_GROUP_START(rltrp)
LOG_ADD(LOG_FLOAT, x, &relative_pos[0])
LOG_ADD(LOG_FLOAT, y, &relative_pos[1])
LOG_ADD(LOG_FLOAT, z, &relative_pos[2])
LOG_ADD(LOG_UINT8, sm, &log_set_motors)
LOG_GROUP_STOP(rltrp)

LOG_GROUP_START(rlttp)
LOG_ADD(LOG_FLOAT, x, &target_pos[0])
LOG_ADD(LOG_FLOAT, y, &target_pos[1])
LOG_ADD(LOG_FLOAT, z, &target_pos[2])
LOG_GROUP_STOP(rltrp)

LOG_GROUP_START(rltte)
LOG_ADD(LOG_FLOAT, x, &pos_error[0])
LOG_ADD(LOG_FLOAT, y, &pos_error[1])
LOG_ADD(LOG_FLOAT, z, &pos_error[2])
LOG_GROUP_STOP(rltre)