AP_HAL_PX4: removal of legacy UAVCAN support

This commit is contained in:
Eugene Shamaev 2017-04-13 20:49:29 +03:00 committed by Andrew Tridgell
parent b3d9126407
commit 0c4a68314b
4 changed files with 0 additions and 65 deletions

View File

@ -78,9 +78,6 @@ void PX4RCOutput::init()
// publish actuator vaules on demand
_actuator_direct_pub = nullptr;
// and armed state
_actuator_armed_pub = nullptr;
}
@ -421,59 +418,6 @@ void PX4RCOutput::_arm_actuators(bool arm)
}
_armed.lockdown = false;
_armed.force_failsafe = false;
if (_actuator_armed_pub == nullptr) {
_actuator_armed_pub = orb_advertise(ORB_ID(actuator_armed), &_armed);
} else {
orb_publish(ORB_ID(actuator_armed), _actuator_armed_pub, &_armed);
}
}
/*
publish new outputs to the actuator_direct topic
*/
void PX4RCOutput::_publish_actuators(void)
{
struct actuator_direct_s actuators;
if (_esc_pwm_min == 0 ||
_esc_pwm_max == 0) {
// not initialised yet
return;
}
actuators.nvalues = _max_channel;
if (actuators.nvalues > actuators.NUM_ACTUATORS_DIRECT) {
actuators.nvalues = actuators.NUM_ACTUATORS_DIRECT;
}
// don't publish more than 8 actuators for now, as the uavcan ESC
// driver refuses to update any motors if you try to publish more
// than 8
if (actuators.nvalues > 8) {
actuators.nvalues = 8;
}
bool armed = hal.util->get_soft_armed();
actuators.timestamp = hrt_absolute_time();
for (uint8_t i=0; i<actuators.nvalues; i++) {
if (!armed) {
actuators.values[i] = 0;
} else {
actuators.values[i] = (_period[i] - _esc_pwm_min) / (float)(_esc_pwm_max - _esc_pwm_min);
}
// actuator values are from -1 to 1
actuators.values[i] = actuators.values[i]*2 - 1;
}
if (_actuator_direct_pub == nullptr) {
_actuator_direct_pub = orb_advertise(ORB_ID(actuator_direct), &actuators);
} else {
orb_publish(ORB_ID(actuator_direct), _actuator_direct_pub, &actuators);
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
_arm_actuators(true);
} else {
_arm_actuators(false);
}
}
void PX4RCOutput::_send_outputs(void)
@ -537,8 +481,6 @@ void PX4RCOutput::_send_outputs(void)
if(AP_BoardConfig::get_can_enable() >= 1)
{
// also publish to actuator_direct (UAVCAN is published via AP_UAVCAN)
_publish_actuators();
#if HAL_WITH_UAVCAN
if(hal.can_mgr != nullptr)

View File

@ -3,7 +3,6 @@
#include "AP_HAL_PX4.h"
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#define PX4_NUM_OUTPUT_CHANNELS 16
@ -63,7 +62,6 @@ private:
actuator_armed_s _armed;
orb_advert_t _actuator_direct_pub = nullptr;
orb_advert_t _actuator_armed_pub = nullptr;
uint16_t _esc_pwm_min = 0;
uint16_t _esc_pwm_max = 0;

View File

@ -52,7 +52,6 @@
#include "AP_HAL_PX4.h"
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
namespace PX4 {

View File

@ -16,13 +16,9 @@
#include "uORB/uORB.h"
#include "uORB/topics/parameter_update.h"
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);
ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
param_t param_find(const char *name)
{