mirror of https://github.com/ArduPilot/ardupilot
AP_UAVCAN: Change for servo and ESC interface from RCOutput to SRV_Channels
This commit is contained in:
Eugene Shamaev
Andrew Tridgell
parent
afce24e45d
commit
1a888c16f4
|
|
@ -348,8 +348,8 @@ AP_UAVCAN::AP_UAVCAN() :
|
|||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
|
||||
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
|
||||
_rco_conf[i].active = false;
|
||||
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
||||
_SRV_conf[i].active = false;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
|
||||
|
|
@ -387,7 +387,7 @@ AP_UAVCAN::AP_UAVCAN() :
|
|||
_bi_BM_listeners[i] = nullptr;
|
||||
}
|
||||
|
||||
_rc_out_sem = hal.util->new_semaphore();
|
||||
SRV_sem = hal.util->new_semaphore();
|
||||
_led_out_sem = hal.util->new_semaphore();
|
||||
|
||||
debug_uavcan(2, "AP_UAVCAN constructed\n\r");
|
||||
|
|
@ -537,21 +537,21 @@ bool AP_UAVCAN::try_init(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
bool AP_UAVCAN::rc_out_sem_take()
|
||||
bool AP_UAVCAN::SRV_sem_take()
|
||||
{
|
||||
bool sem_ret = _rc_out_sem->take(10);
|
||||
bool sem_ret = SRV_sem->take(10);
|
||||
if (!sem_ret) {
|
||||
debug_uavcan(1, "AP_UAVCAN RCOut semaphore fail\n\r");
|
||||
debug_uavcan(1, "AP_UAVCAN SRV semaphore fail\n\r");
|
||||
}
|
||||
return sem_ret;
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rc_out_sem_give()
|
||||
void AP_UAVCAN::SRV_sem_give()
|
||||
{
|
||||
_rc_out_sem->give();
|
||||
SRV_sem->give();
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rc_out_send_servos(void)
|
||||
void AP_UAVCAN::SRV_send_servos(void)
|
||||
{
|
||||
uint8_t starting_servo = 0;
|
||||
bool repeat_send;
|
||||
|
|
@ -562,7 +562,7 @@ void AP_UAVCAN::rc_out_send_servos(void)
|
|||
|
||||
uint8_t i;
|
||||
// UAVCAN can hold maximum of 15 commands in one frame
|
||||
for (i = 0; starting_servo < UAVCAN_RCO_NUMBER && i < 15; starting_servo++) {
|
||||
for (i = 0; starting_servo < UAVCAN_SRV_NUMBER && i < 15; starting_servo++) {
|
||||
uavcan::equipment::actuator::Command cmd;
|
||||
|
||||
/*
|
||||
|
|
@ -574,14 +574,14 @@ void AP_UAVCAN::rc_out_send_servos(void)
|
|||
* physically possible throws at [-1:1] limits.
|
||||
*/
|
||||
|
||||
if (_rco_conf[starting_servo].active && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
|
||||
if (_SRV_conf[starting_servo].active && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
|
||||
cmd.actuator_id = starting_servo + 1;
|
||||
|
||||
// TODO: other types
|
||||
cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
|
||||
|
||||
// TODO: failsafe, safety
|
||||
cmd.command_value = constrain_float(((float) _rco_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
|
||||
cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
|
||||
|
||||
msg.commands.push_back(cmd);
|
||||
|
||||
|
|
@ -599,7 +599,7 @@ void AP_UAVCAN::rc_out_send_servos(void)
|
|||
} while (repeat_send);
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rc_out_send_esc(void)
|
||||
void AP_UAVCAN::SRV_send_esc(void)
|
||||
{
|
||||
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
|
||||
uavcan::equipment::esc::RawCommand esc_msg;
|
||||
|
|
@ -608,10 +608,10 @@ void AP_UAVCAN::rc_out_send_esc(void)
|
|||
uint8_t k = 0;
|
||||
|
||||
// find out how many esc we have enabled and if they are active at all
|
||||
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
|
||||
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
||||
if ((((uint32_t) 1) << i) & _esc_bm) {
|
||||
max_esc_num = i + 1;
|
||||
if (_rco_conf[i].active) {
|
||||
if (_SRV_conf[i].active) {
|
||||
active_esc_num++;
|
||||
}
|
||||
}
|
||||
|
|
@ -626,7 +626,7 @@ void AP_UAVCAN::rc_out_send_esc(void)
|
|||
|
||||
if ((((uint32_t) 1) << i) & _esc_bm) {
|
||||
// TODO: ESC negative scaling for reverse thrust and reverse rotation
|
||||
float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_rco_conf[i].pulse) + 1.0) / 2.0;
|
||||
float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
|
||||
|
||||
scaled = constrain_float(scaled, 0, cmd_max);
|
||||
|
||||
|
|
@ -649,42 +649,42 @@ void AP_UAVCAN::do_cyclic(void)
|
|||
return;
|
||||
}
|
||||
|
||||
auto *node = get_node();
|
||||
auto *node = get_node();
|
||||
|
||||
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));
|
||||
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));
|
||||
|
||||
if (error < 0) {
|
||||
hal.scheduler->delay_microseconds(1000);
|
||||
return;
|
||||
}
|
||||
if (error < 0) {
|
||||
hal.scheduler->delay_microseconds(1000);
|
||||
return;
|
||||
}
|
||||
|
||||
if (rc_out_sem_take()) {
|
||||
if (SRV_sem_take()) {
|
||||
|
||||
if (_rco_armed) {
|
||||
if (_SRV_armed) {
|
||||
|
||||
// if we have any Servos in bitmask
|
||||
if (_servo_bm > 0) {
|
||||
rc_out_send_servos();
|
||||
}
|
||||
// if we have any Servos in bitmask
|
||||
if (_servo_bm > 0) {
|
||||
SRV_send_servos();
|
||||
}
|
||||
|
||||
// if we have any ESC's in bitmask
|
||||
if (_esc_bm > 0) {
|
||||
rc_out_send_esc();
|
||||
}
|
||||
}
|
||||
// if we have any ESC's in bitmask
|
||||
if (_esc_bm > 0) {
|
||||
SRV_send_esc();
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
|
||||
// mark as transmitted
|
||||
_rco_conf[i].active = false;
|
||||
}
|
||||
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
||||
// mark as transmitted
|
||||
_SRV_conf[i].active = false;
|
||||
}
|
||||
|
||||
rc_out_sem_give();
|
||||
}
|
||||
SRV_sem_give();
|
||||
}
|
||||
|
||||
if (led_out_sem_take()) {
|
||||
|
||||
led_out_send();
|
||||
led_out_sem_give();
|
||||
led_out_send();
|
||||
led_out_sem_give();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -748,43 +748,43 @@ uavcan::Node<0> *AP_UAVCAN::get_node()
|
|||
return _node;
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rco_set_safety_pwm(uint32_t chmask, uint16_t pulse_len)
|
||||
void AP_UAVCAN::SRV_set_safety_pwm(uint32_t chmask, uint16_t pulse_len)
|
||||
{
|
||||
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
|
||||
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
||||
if (chmask & (((uint32_t) 1) << i)) {
|
||||
_rco_conf[i].safety_pulse = pulse_len;
|
||||
_SRV_conf[i].safety_pulse = pulse_len;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rco_set_failsafe_pwm(uint32_t chmask, uint16_t pulse_len)
|
||||
void AP_UAVCAN::SRV_set_failsafe_pwm(uint32_t chmask, uint16_t pulse_len)
|
||||
{
|
||||
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
|
||||
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
||||
if (chmask & (((uint32_t) 1) << i)) {
|
||||
_rco_conf[i].failsafe_pulse = pulse_len;
|
||||
_SRV_conf[i].failsafe_pulse = pulse_len;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rco_force_safety_on(void)
|
||||
void AP_UAVCAN::SRV_force_safety_on(void)
|
||||
{
|
||||
_rco_safety = true;
|
||||
_SRV_safety = true;
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rco_force_safety_off(void)
|
||||
void AP_UAVCAN::SRV_force_safety_off(void)
|
||||
{
|
||||
_rco_safety = false;
|
||||
_SRV_safety = false;
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rco_arm_actuators(bool arm)
|
||||
void AP_UAVCAN::SRV_arm_actuators(bool arm)
|
||||
{
|
||||
_rco_armed = arm;
|
||||
_SRV_armed = arm;
|
||||
}
|
||||
|
||||
void AP_UAVCAN::rco_write(uint16_t pulse_len, uint8_t ch)
|
||||
void AP_UAVCAN::SRV_write(uint16_t pulse_len, uint8_t ch)
|
||||
{
|
||||
_rco_conf[ch].pulse = pulse_len;
|
||||
_rco_conf[ch].active = true;
|
||||
_SRV_conf[ch].pulse = pulse_len;
|
||||
_SRV_conf[ch].active = true;
|
||||
}
|
||||
|
||||
uint8_t AP_UAVCAN::find_gps_without_listener(void)
|
||||
|
|
|
|||
|
|
@ -28,8 +28,8 @@
|
|||
#define UAVCAN_NODE_POOL_BLOCK_SIZE 256
|
||||
#endif
|
||||
|
||||
#ifndef UAVCAN_RCO_NUMBER
|
||||
#define UAVCAN_RCO_NUMBER 18
|
||||
#ifndef UAVCAN_SRV_NUMBER
|
||||
#define UAVCAN_SRV_NUMBER 18
|
||||
#endif
|
||||
|
||||
#define AP_UAVCAN_MAX_LISTENERS 4
|
||||
|
|
@ -119,8 +119,8 @@ public:
|
|||
void update_bi_state(uint8_t id);
|
||||
|
||||
// synchronization for RC output
|
||||
bool rc_out_sem_take();
|
||||
void rc_out_sem_give();
|
||||
bool SRV_sem_take();
|
||||
void SRV_sem_give();
|
||||
|
||||
// synchronization for LED output
|
||||
bool led_out_sem_take();
|
||||
|
|
@ -128,8 +128,8 @@ public:
|
|||
void led_out_send();
|
||||
|
||||
// output from do_cyclic
|
||||
void rc_out_send_servos();
|
||||
void rc_out_send_esc();
|
||||
void SRV_send_servos();
|
||||
void SRV_send_esc();
|
||||
|
||||
private:
|
||||
// ------------------------- GPS
|
||||
|
|
@ -173,11 +173,11 @@ private:
|
|||
uint16_t safety_pulse;
|
||||
uint16_t failsafe_pulse;
|
||||
bool active;
|
||||
} _rco_conf[UAVCAN_RCO_NUMBER];
|
||||
} _SRV_conf[UAVCAN_SRV_NUMBER];
|
||||
|
||||
bool _initialized;
|
||||
uint8_t _rco_armed;
|
||||
uint8_t _rco_safety;
|
||||
uint8_t _SRV_armed;
|
||||
uint8_t _SRV_safety;
|
||||
|
||||
typedef struct {
|
||||
bool enabled;
|
||||
|
|
@ -192,7 +192,7 @@ private:
|
|||
uint64_t last_update;
|
||||
} _led_conf;
|
||||
|
||||
AP_HAL::Semaphore *_rc_out_sem;
|
||||
AP_HAL::Semaphore *SRV_sem;
|
||||
AP_HAL::Semaphore *_led_out_sem;
|
||||
|
||||
class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable {
|
||||
|
|
@ -263,12 +263,12 @@ public:
|
|||
void do_cyclic(void);
|
||||
bool try_init(void);
|
||||
|
||||
void rco_set_safety_pwm(uint32_t chmask, uint16_t pulse_len);
|
||||
void rco_set_failsafe_pwm(uint32_t chmask, uint16_t pulse_len);
|
||||
void rco_force_safety_on(void);
|
||||
void rco_force_safety_off(void);
|
||||
void rco_arm_actuators(bool arm);
|
||||
void rco_write(uint16_t pulse_len, uint8_t ch);
|
||||
void SRV_set_safety_pwm(uint32_t chmask, uint16_t pulse_len);
|
||||
void SRV_set_failsafe_pwm(uint32_t chmask, uint16_t pulse_len);
|
||||
void SRV_force_safety_on(void);
|
||||
void SRV_force_safety_off(void);
|
||||
void SRV_arm_actuators(bool arm);
|
||||
void SRV_write(uint16_t pulse_len, uint8_t ch);
|
||||
bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);
|
||||
|
||||
void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr)
|
||||
|
|
|
|||
Loading…
Reference in New Issue