zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Added actuator control removal

sbg
Lorenz Meier 11 years ago
parent
c46bd8b041 63d81ba415
commit
c311462f3c
9 changed files with 41 additions and 200 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 23
      src/drivers/ardrone_interface/ardrone_motor_control.c
  2. 14
      src/drivers/mkblctrl/mkblctrl.cpp
  3. 20
      src/drivers/px4fmu/fmu.cpp
  4. 56
      src/drivers/px4io/px4io.cpp
  5. 34
      src/modules/mavlink/orb_listener.c
  6. 6
      src/modules/px4iofirmware/mixer.cpp
  7. 21
      src/modules/sdlog2/sdlog2.c
  8. 7
      src/modules/uORB/objects_common.cpp
  9. 60
      src/modules/uORB/topics/actuator_controls_effective.h

23
src/drivers/ardrone_interface/ardrone_motor_control.c
Unescape View File

@ -46,7 +46,6 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include "ardrone_motor_control.h" #include "ardrone_motor_control.h"
@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
static bool initialized = false; static bool initialized = false;
/* publish effective outputs */
static struct actuator_controls_effective_s actuator_controls_effective;
static orb_advert_t actuator_controls_effective_pub;
/* linearly scale the control inputs from 0 to startpoint_full_control */ /* linearly scale the control inputs from 0 to startpoint_full_control */
if (motor_thrust < startpoint_full_control) { if (motor_thrust < startpoint_full_control) {
@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
} }
/* publish effective outputs */
actuator_controls_effective.control_effective[0] = roll_control;
actuator_controls_effective.control_effective[1] = pitch_control;
/* yaw output after limiting */
actuator_controls_effective.control_effective[2] = yaw_control;
/* possible motor thrust limiting */
actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
if (!initialized) {
/* advertise and publish */
actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
initialized = true;
} else {
/* already initialized, just publishing */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
}
/* set the motor values */ /* set the motor values */
/* scale up from 0..1 to 10..500) */ /* scale up from 0..1 to 10..500) */

14
src/drivers/mkblctrl/mkblctrl.cpp
Unescape View File

@ -73,7 +73,6 @@
#include <drivers/drv_mixer.h> #include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h> #include <uORB/topics/esc_status.h>
@ -143,7 +142,6 @@ private:
int _frametype; int _frametype;
orb_advert_t _t_outputs; orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
orb_advert_t _t_esc_status; orb_advert_t _t_esc_status;
unsigned int _num_outputs; unsigned int _num_outputs;
@ -252,7 +250,6 @@ MK::MK(int bus) :
_t_actuators(-1), _t_actuators(-1),
_t_actuator_armed(-1), _t_actuator_armed(-1),
_t_outputs(0), _t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0), _t_esc_status(0),
_num_outputs(0), _num_outputs(0),
_motortest(false), _motortest(false),
@ -525,13 +522,6 @@ MK::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs); &outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
/* advertise the blctrl status */ /* advertise the blctrl status */
esc_status_s esc; esc_status_s esc;
memset(&esc, 0, sizeof(esc)); memset(&esc, 0, sizeof(esc));
@ -595,9 +585,6 @@ MK::task_main()
outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
outputs.timestamp = hrt_absolute_time(); outputs.timestamp = hrt_absolute_time();
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
/* iterate actuators */ /* iterate actuators */
for (unsigned int i = 0; i < _num_outputs; i++) { for (unsigned int i = 0; i < _num_outputs; i++) {
@ -701,7 +688,6 @@ MK::task_main()
//::close(_t_esc_status); //::close(_t_esc_status);
::close(_t_actuators); ::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed); ::close(_t_actuator_armed);

20
src/drivers/px4fmu/fmu.cpp
Unescape View File

@ -69,7 +69,6 @@
#include <drivers/drv_rc_input.h> #include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
@ -123,7 +122,6 @@ private:
int _t_actuators; int _t_actuators;
int _t_actuator_armed; int _t_actuator_armed;
orb_advert_t _t_outputs; orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs; unsigned _num_outputs;
bool _primary_pwm_device; bool _primary_pwm_device;
@ -220,7 +218,6 @@ PX4FMU::PX4FMU() :
_t_actuators(-1), _t_actuators(-1),
_t_actuator_armed(-1), _t_actuator_armed(-1),
_t_outputs(0), _t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0), _num_outputs(0),
_primary_pwm_device(false), _primary_pwm_device(false),
_task_should_exit(false), _task_should_exit(false),
@ -471,13 +468,6 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs); &outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
pollfd fds[2]; pollfd fds[2];
fds[0].fd = _t_actuators; fds[0].fd = _t_actuators;
fds[0].events = POLLIN; fds[0].events = POLLIN;
@ -550,7 +540,7 @@ PX4FMU::task_main()
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */ /* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */ /* can we mix? */
if (_mixers != nullptr) { if (_mixers != nullptr) {
@ -599,13 +589,6 @@ PX4FMU::task_main()
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output actual limited values */
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */ /* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) { for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]); up_pwm_servo_set(i, pwm_limited[i]);
@ -670,7 +653,6 @@ PX4FMU::task_main()
} }
::close(_t_actuators); ::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed); ::close(_t_actuator_armed);
/* make sure servos are off */ /* make sure servos are off */

56
src/drivers/px4io/px4io.cpp
Unescape View File

@ -73,7 +73,6 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h> #include <uORB/topics/safety.h>
@ -260,14 +259,12 @@ private:
/* advertised topics */ /* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io orb_advert_t _to_input_rc; ///< rc inputs from io
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_battery; ///< battery status / voltage
orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs actuator_outputs_s _outputs; ///<mixed outputs
actuator_controls_effective_s _controls_effective; ///<effective controls
bool _primary_pwm_device; ///<true if we are the default PWM output bool _primary_pwm_device; ///<true if we are the default PWM output
@ -330,11 +327,6 @@ private:
*/ */
int io_publish_raw_rc(); int io_publish_raw_rc();
/**
* Fetch and publish the mixed control values.
*/
int io_publish_mixed_controls();
/** /**
* Fetch and publish the PWM servo outputs. * Fetch and publish the PWM servo outputs.
*/ */
@ -475,7 +467,6 @@ PX4IO::PX4IO(device::Device *interface) :
_t_param(-1), _t_param(-1),
_t_vehicle_command(-1), _t_vehicle_command(-1),
_to_input_rc(0), _to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0), _to_outputs(0),
_to_battery(0), _to_battery(0),
_to_servorail(0), _to_servorail(0),
@ -843,8 +834,7 @@ PX4IO::task_main()
/* get raw R/C input from IO */ /* get raw R/C input from IO */
io_publish_raw_rc(); io_publish_raw_rc();
/* fetch mixed servo controls and PWM outputs from IO */ /* fetch PWM outputs from IO */
io_publish_mixed_controls();
io_publish_pwm_outputs(); io_publish_pwm_outputs();
} }
@ -1366,50 +1356,6 @@ PX4IO::io_publish_raw_rc()
return OK; return OK;
} }
int
PX4IO::io_publish_mixed_controls()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* if not taking raw PPM from us, must be mixing */
if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
return OK;
/* data we are going to fetch */
actuator_controls_effective_s controls_effective;
controls_effective.timestamp = hrt_absolute_time();
/* get actuator controls from IO */
uint16_t act[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
if (ret != OK)
return ret;
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++)
controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
/* laxily advertise on first publication */
if (_to_actuators_effective == 0) {
_to_actuators_effective =
orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
&controls_effective);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
_to_actuators_effective, &controls_effective);
}
return OK;
}
int int
PX4IO::io_publish_pwm_outputs() PX4IO::io_publish_pwm_outputs()
{ {

34
src/modules/mavlink/orb_listener.c
Unescape View File

@ -72,7 +72,6 @@ struct vehicle_status_s v_status;
struct rc_channels_s rc; struct rc_channels_s rc;
struct rc_input_values rc_raw; struct rc_input_values rc_raw;
struct actuator_armed_s armed; struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0; struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
struct airspeed_s airspeed; struct airspeed_s airspeed;
@ -119,7 +118,6 @@ static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l); static void l_actuator_outputs(const struct listener *l);
static void l_actuator_armed(const struct listener *l); static void l_actuator_armed(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l);
static void l_vehicle_attitude_controls_effective(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l); static void l_debug_key_value(const struct listener *l);
static void l_optical_flow(const struct listener *l); static void l_optical_flow(const struct listener *l);
@ -147,7 +145,6 @@ static const struct listener listeners[] = {
{l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_actuator_armed, &mavlink_subs.armed_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
@ -249,7 +246,7 @@ l_vehicle_attitude(const struct listener *l)
last_sent_vfr = t; last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
} }
} }
@ -603,32 +600,6 @@ l_manual_control_setpoint(const struct listener *l)
0); 0);
} }
void
l_vehicle_attitude_controls_effective(const struct listener *l)
{
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl0 ",
actuators_effective_0.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
actuators_effective_0.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
actuators_effective_0.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
actuators_effective_0.control_effective[3]);
}
}
void void
l_vehicle_attitude_controls(const struct listener *l) l_vehicle_attitude_controls(const struct listener *l)
{ {
@ -839,9 +810,6 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */ /* --- ACTUATOR CONTROL VALUE --- */
mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */

6
src/modules/px4iofirmware/mixer.cpp
Unescape View File

@ -185,7 +185,7 @@ mixer_tick(void)
r_page_servos[i] = r_page_servo_failsafe[i]; r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */ /* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
} }
@ -201,6 +201,10 @@ mixer_tick(void)
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0; r_page_servos[i] = 0;
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
} }
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {

21
src/modules/sdlog2/sdlog2.c
Unescape View File

@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp; struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs; struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls; struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
struct vehicle_local_position_s local_pos; struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos; struct vehicle_global_position_s global_pos;
@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[])
int rates_sp_sub; int rates_sp_sub;
int act_outputs_sub; int act_outputs_sub;
int act_controls_sub; int act_controls_sub;
int act_controls_effective_sub;
int local_pos_sub; int local_pos_sub;
int local_pos_sp_sub; int local_pos_sp_sub;
int global_pos_sub; int global_pos_sub;
@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body)); memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */ /* number of subscriptions */
const ssize_t fdsc = 20; const ssize_t fdsc = 19;
/* Sanity check variable and index */ /* sanity check variable and index */
ssize_t fdsc_count = 0; ssize_t fdsc_count = 0;
/* file descriptors to wait for */ /* file descriptors to wait for */
struct pollfd fds[fdsc]; struct pollfd fds[fdsc];
@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN; fds[fdsc_count].events = POLLIN;
fdsc_count++; fdsc_count++;
/* --- ACTUATOR CONTROL EFFECTIVE --- */
subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
fds[fdsc_count].fd = subs.act_controls_effective_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */ /* --- LOCAL POSITION --- */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub; fds[fdsc_count].fd = subs.local_pos_sub;
@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ATTC); LOGBUFFER_WRITE_AND_COUNT(ATTC);
} }
/* --- ACTUATOR CONTROL EFFECTIVE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
// TODO not implemented yet
}
/* --- LOCAL POSITION --- */ /* --- LOCAL POSITION --- */
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);

7
src/modules/uORB/objects_common.cpp
Unescape View File

@ -169,13 +169,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
#include "topics/actuator_armed.h" #include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s);
/* actuator controls, as set by actuators / mixers after limiting */
#include "topics/actuator_controls_effective.h"
ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
#include "topics/actuator_outputs.h" #include "topics/actuator_outputs.h"
ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);

60
src/modules/uORB/topics/actuator_controls_effective.h
Unescape View File

@ -46,34 +46,34 @@
#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
#include <stdint.h> //#include <stdint.h>
#include "../uORB.h" //#include "../uORB.h"
#include "actuator_controls.h" //#include "actuator_controls.h"
//
//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
//
///**
// * @addtogroup topics
// * @{
// */
//
//struct actuator_controls_effective_s {
// uint64_t timestamp;
// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
//};
//
///**
// * @}
// */
//
///* actuator control sets; this list can be expanded as more controllers emerge */
//ORB_DECLARE(actuator_controls_effective_0);
//ORB_DECLARE(actuator_controls_effective_1);
//ORB_DECLARE(actuator_controls_effective_2);
//ORB_DECLARE(actuator_controls_effective_3);
//
///* control sets with pre-defined applications */
//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0)
#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS #endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
/**
* @addtogroup topics
* @{
*/
struct actuator_controls_effective_s {
uint64_t timestamp;
float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
};
/**
* @}
*/
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_effective_0);
ORB_DECLARE(actuator_controls_effective_1);
ORB_DECLARE(actuator_controls_effective_2);
ORB_DECLARE(actuator_controls_effective_3);
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0)
#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */

Write Preview
Loading…
Cancel
Save