Browse Source

actuator controls as msg

sbg
Thomas Gubler 10 years ago
parent
commit
dc945a3f3f
  1. 4
      msg/px4_msgs/actuator_controls.msg
  2. 3
      msg/templates/msg.h.template
  3. 2
      src/modules/mc_att_control/mc_att_control_base.h
  4. 6
      src/modules/mc_att_control/mc_att_control_main.cpp
  5. 38
      src/modules/uORB/topics/actuator_controls.h
  6. 7
      src/platforms/px4_defines.h
  7. 4
      src/platforms/px4_includes.h
  8. 1
      src/systemcmds/esc_calib/esc_calib.c

4
msg/px4_msgs/actuator_controls.msg

@ -0,0 +1,4 @@ @@ -0,0 +1,4 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
float32[8] control

3
msg/templates/msg.h.template

@ -104,7 +104,8 @@ type_map = {'int8': 'int8_t', @@ -104,7 +104,8 @@ type_map = {'int8': 'int8_t',
'uint32': 'uint32_t',
'uint64': 'uint64_t',
'float32': 'float',
'bool': 'bool'}
'bool': 'bool',
'actuator_controls': 'actuator_controls'}
# Function to print a standard ros type
def print_field_def(field):

2
src/modules/mc_att_control/mc_att_control_base.h

@ -53,11 +53,11 @@ @@ -53,11 +53,11 @@
#include <errno.h>
#include <math.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/perf_counter.h>
#include <lib/mathlib/mathlib.h>

6
src/modules/mc_att_control/mc_att_control_main.cpp

@ -61,12 +61,6 @@ @@ -61,12 +61,6 @@
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>

38
src/modules/uORB/topics/actuator_controls.h

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,48 +31,32 @@ @@ -31,48 +31,32 @@
*
****************************************************************************/
/**
* @file actuator_controls.h
*
* Actuator control topics - mixer inputs.
*
* Values published to these topics are the outputs of the vehicle control
* system, and are expected to be mixed and used to drive the actuators
* (servos, speed controls, etc.) that operate the vehicle.
*
* Each topic can be published by a single controller
*/
/* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_controls.msg */
#ifndef TOPIC_ACTUATOR_CONTROLS_H
#define TOPIC_ACTUATOR_CONTROLS_H
#pragma once
#include <stdint.h>
#include <platforms/px4_defines.h>
#include "../uORB.h"
#define NUM_ACTUATOR_CONTROLS 8
#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
#define NUM_ACTUATOR_CONTROLS 8
#define NUM_ACTUATOR_CONTROL_GROUPS 4
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
/**
* @addtogroup topics
* @{
*/
struct actuator_controls_s {
uint64_t timestamp;
float control[NUM_ACTUATOR_CONTROLS];
float control[8];
};
/**
* @}
*/
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
#endif
/* register this as object request broker structure */
ORB_DECLARE(actuator_controls);

7
src/platforms/px4_defines.h

@ -139,3 +139,10 @@ typedef param_t px4_param_t; @@ -139,3 +139,10 @@ typedef param_t px4_param_t;
/* wrapper for rotation matrices stored in arrays */
#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
/* Diverese uORB header defiens */ //XXX: move to better location
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);

4
src/platforms/px4_includes.h

@ -57,6 +57,10 @@ @@ -57,6 +57,10 @@
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>

1
src/systemcmds/esc_calib/esc_calib.c

@ -39,6 +39,7 @@ @@ -39,6 +39,7 @@
*/
#include <nuttx/config.h>
#include <platforms/px4_defines.h>
#include <stdio.h>
#include <stdlib.h>

Loading…
Cancel
Save