mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
AP_HAL_PX4: removal of legacy UAVCAN support
This commit is contained in:
parent
b3d9126407
commit
0c4a68314b
@ -78,9 +78,6 @@ void PX4RCOutput::init()
|
|||||||
|
|
||||||
// publish actuator vaules on demand
|
// publish actuator vaules on demand
|
||||||
_actuator_direct_pub = nullptr;
|
_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.lockdown = false;
|
||||||
_armed.force_failsafe = 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)
|
void PX4RCOutput::_send_outputs(void)
|
||||||
@ -537,8 +481,6 @@ void PX4RCOutput::_send_outputs(void)
|
|||||||
|
|
||||||
if(AP_BoardConfig::get_can_enable() >= 1)
|
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_WITH_UAVCAN
|
||||||
|
|
||||||
if(hal.can_mgr != nullptr)
|
if(hal.can_mgr != nullptr)
|
||||||
|
@ -3,7 +3,6 @@
|
|||||||
#include "AP_HAL_PX4.h"
|
#include "AP_HAL_PX4.h"
|
||||||
#include <systemlib/perf_counter.h>
|
#include <systemlib/perf_counter.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
#include <uORB/topics/actuator_armed.h>
|
|
||||||
|
|
||||||
#define PX4_NUM_OUTPUT_CHANNELS 16
|
#define PX4_NUM_OUTPUT_CHANNELS 16
|
||||||
|
|
||||||
@ -63,7 +62,6 @@ private:
|
|||||||
actuator_armed_s _armed;
|
actuator_armed_s _armed;
|
||||||
|
|
||||||
orb_advert_t _actuator_direct_pub = nullptr;
|
orb_advert_t _actuator_direct_pub = nullptr;
|
||||||
orb_advert_t _actuator_armed_pub = nullptr;
|
|
||||||
uint16_t _esc_pwm_min = 0;
|
uint16_t _esc_pwm_min = 0;
|
||||||
uint16_t _esc_pwm_max = 0;
|
uint16_t _esc_pwm_max = 0;
|
||||||
|
|
||||||
|
@ -52,7 +52,6 @@
|
|||||||
#include "AP_HAL_PX4.h"
|
#include "AP_HAL_PX4.h"
|
||||||
#include <systemlib/perf_counter.h>
|
#include <systemlib/perf_counter.h>
|
||||||
#include <uORB/topics/actuator_armed.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
|
||||||
|
|
||||||
namespace PX4 {
|
namespace PX4 {
|
||||||
|
|
||||||
|
@ -16,13 +16,9 @@
|
|||||||
|
|
||||||
#include "uORB/uORB.h"
|
#include "uORB/uORB.h"
|
||||||
#include "uORB/topics/parameter_update.h"
|
#include "uORB/topics/parameter_update.h"
|
||||||
#include <uORB/topics/uavcan_parameter_request.h>
|
|
||||||
#include <uORB/topics/uavcan_parameter_value.h>
|
|
||||||
|
|
||||||
/** parameter update topic */
|
/** parameter update topic */
|
||||||
ORB_DEFINE(parameter_update, struct parameter_update_s);
|
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)
|
param_t param_find(const char *name)
|
||||||
{
|
{
|
||||||
|
Loading…
Reference in New Issue
Block a user