rt_interface_lcm.cpp File Reference

#include <pthread.h>
#include <rt/rt_interface_lcm.h>
#include <rt/rt_sbus.h>
#include <gui_main_control_settings_t.hpp>
#include <rc_channels_t.hpp>

Include dependency graph for rt_interface_lcm.cpp:

Go to the source code of this file.

Functions
void	control_iteration_lcm ()
	Increments the number of control iterations since the last LCM packet. More...
int	get_iterations_since_last_lcm ()
	Gets the iterations since the last lcm. More...
void	get_main_control_settings (void *settings)
void	get_rc_channels (void *settings)
void	sbus_packet_complete ()
	Function which handles the completion of an SBUS Packet and overrides the LCM control settings as desired. More...
void	init_interface_lcm (lcm::LCM *main_lcm)
	Initializer for all handlers related to the interface LCM streams. More...
void *	v_memcpy (void *dest, volatile void *src, size_t n)

Variables
static lcm::LCM *	g_lcm
static pthread_mutex_t	lcm_get_set_mutex
int	iterations_since_last_lcm = 0
gui_main_control_settings_t	main_control_settings
rc_channels_t	rc_channels

Function Documentation

void control_iteration_lcm

(

)

Increments the number of control iterations since the last LCM packet.

Definition at line 18 of file rt_interface_lcm.cpp.

References iterations_since_last_lcm.

{ iterations_since_last_lcm++; }

int get_iterations_since_last_lcm

(

)

Gets the iterations since the last lcm.

Returns

The iterations since last lcm.

Definition at line 25 of file rt_interface_lcm.cpp.

References iterations_since_last_lcm.

{ return iterations_since_last_lcm; }

void get_main_control_settings

(

void *

settings

)

Definition at line 62 of file rt_interface_lcm.cpp.

References lcm_get_set_mutex, main_control_settings, and v_memcpy().

                                               {
  pthread_mutex_lock(&lcm_get_set_mutex);
  v_memcpy(settings, &main_control_settings, sizeof(main_control_settings));
  pthread_mutex_unlock(&lcm_get_set_mutex);
}

Here is the call graph for this function:

void get_rc_channels

(

void *

settings

)

Definition at line 68 of file rt_interface_lcm.cpp.

References lcm_get_set_mutex, rc_channels, and v_memcpy().

                                     {
  pthread_mutex_lock(&lcm_get_set_mutex);
  v_memcpy(settings, &rc_channels, sizeof(rc_channels));
  pthread_mutex_unlock(&lcm_get_set_mutex);
}

Here is the call graph for this function:

void init_interface_lcm

(

lcm::LCM *

main_lcm

)

Initializer for all handlers related to the interface LCM streams.

Parameters

main_lcm

A pointer to the main lcm object used in the lcm thread

Definition at line 172 of file rt_interface_lcm.cpp.

References g_lcm, main_control_settings, Handler::main_control_settings_handler(), and Handler::rc_channels_handler().

                                          {
  printf("[RT Interface LCM] Initializing...\n");
  g_lcm = main_lcm;

  Handler handlerObj;

  g_lcm->subscribe("INTERFACE_gui_main_control_settings",
                   &Handler::main_control_settings_handler, &handlerObj);
  g_lcm->subscribe("INTERFACE_rc_channels", &Handler::rc_channels_handler,
                   &handlerObj);

  main_control_settings.enable = 1;

  printf("[RT Interface LCM] Done\n");

  // printf("Initial PFOOT SETTINGS %lf\n",
  // state_estimator_settings.process_noise_pfoot);
}

Here is the call graph for this function:

void sbus_packet_complete

(

)

Function which handles the completion of an SBUS Packet and overrides the LCM control settings as desired.

Definition at line 78 of file rt_interface_lcm.cpp.

References f(), lcm_get_set_mutex, main_control_settings, and read_sbus_channel().

                            {
  pthread_mutex_lock(&lcm_get_set_mutex);

  int ch1 = read_sbus_channel(0);
  int ch2 = read_sbus_channel(1);
  int ch3 = read_sbus_channel(2);
  int ch4 = read_sbus_channel(3);
  int ch7 = read_sbus_channel(6);
  int ch9 = read_sbus_channel(8);
  int ch10 = read_sbus_channel(9);
  int ch11 = read_sbus_channel(10);
  int ch13 = read_sbus_channel(12);

  // velocity scales between 1.0 and 3.0
  float v_scale = 2.0f * (((float)(ch7 - 172)) / 1811.0f) + 1.0f;
  // Ignore commands if switched
  if (ch11 != 1811) {
    // oh shit switch
    if (ch10 == 172) {
      main_control_settings.mode = 0;
    } else if (ch10 == 992) {
      main_control_settings.mode = 12;
    } else if (ch10 == 1811) {
      main_control_settings.mode = 11;
    }

    // Use the joysticks for velocity and yaw control in locomotion gaits
    if (main_control_settings.mode == 11) {
      // Analog channels return a value between ~200 and 1800, with sticks
      // centered at 1000

      if (ch9 == 1811) {
        main_control_settings.variable[0] = 4;
      }  // Stand
      else if (ch9 == 992) {
        main_control_settings.variable[0] = 0;
      }  // Trot
      else if ((ch9 == 172) && (ch13 == 1811)) {
        main_control_settings.variable[0] = 5;
      }  // Trot run
      else if ((ch9 == 172) && (ch13 == 992)) {
        main_control_settings.variable[0] = 2;
      }  // Pronk
      else if ((ch9 == 172) && (ch13 == 172)) {
        main_control_settings.variable[0] = 1;
      }  // Bound

      // If using the stand gait, control orientations
      if (main_control_settings.variable[0] == 4) {
        main_control_settings.rpy_des[0] = ((float)ch4 - 1000) * .0003f;
        main_control_settings.rpy_des[1] = ((float)ch3 - 1000) * .0005f;
        main_control_settings.rpy_des[2] = ((float)ch2 - 1000) * .001f;

        main_control_settings.p_des[0] = 0;
        main_control_settings.p_des[1] = 0;
        main_control_settings.p_des[2] = 0.21 + ((float)ch1 - 1000) * .0001f;
      }
      // For all other gaits, control the velocities
      else {
        main_control_settings.v_des[0] = v_scale * ((float)ch1 - 1000) * .002f;
        main_control_settings.v_des[1] =
            -v_scale * ((float)ch4 - 1000) * .0005f;
        main_control_settings.v_des[2] = 0;
        main_control_settings.p_des[2] = 0.21;

        main_control_settings.omega_des[0] = 0;
        main_control_settings.omega_des[1] = 0;
        main_control_settings.omega_des[2] =
            -v_scale * ((float)ch2 - 1000) * .003f;
      }
    }
    // For standing modes (mpc or balance qp) control orientations
    else if (main_control_settings.mode == 10 ||
             main_control_settings.mode == 3) {
      main_control_settings.rpy_des[0] = ((float)ch4 - 1000) * .0003f;
      main_control_settings.rpy_des[1] = ((float)ch3 - 1000) * .0005f;
      main_control_settings.rpy_des[2] = ((float)ch2 - 1000) * .001f;

      main_control_settings.p_des[0] = 0;
      main_control_settings.p_des[1] = 0;
      main_control_settings.p_des[2] = 0.21 + ((float)ch1 - 1000) * .0001f;
    }
  }
  // Use the joysticks for orientation and height control in standing mode

  pthread_mutex_unlock(&lcm_get_set_mutex);
  // printf("[RT Interface LCM] Got SBUS Packet\n");
}

Here is the call graph for this function:

Here is the caller graph for this function:

void* v_memcpy	(	void *	dest,
		volatile void *	src,
		size_t	n
	)

Definition at line 191 of file rt_interface_lcm.cpp.

                                                         {
  void *src_2 = (void *)src;
  return memcpy(dest, src_2, n);
}

Here is the caller graph for this function:

Variable Documentation

lcm::LCM* g_lcm

static

Definition at line 4 of file rt_interface_lcm.cpp.

int iterations_since_last_lcm = 0

Definition at line 12 of file rt_interface_lcm.cpp.

pthread_mutex_t lcm_get_set_mutex

static

Initial value:

=
    PTHREAD_MUTEX_INITIALIZER

mutex to protect gui settings coming over LCM

Definition at line 8 of file rt_interface_lcm.cpp.

gui_main_control_settings_t main_control_settings

Definition at line 30 of file rt_interface_lcm.cpp.

rc_channels_t rc_channels

Definition at line 34 of file rt_interface_lcm.cpp.