PX4 FMU [12] RC_Channel ［转载］

PX4 FMU [12] RC_Channel

PX4 FMU [12] RC_Channel
                                                                             -------- 转载请注明出处 
                                                                             -------- 更多笔记请访问我的博客：merafour.blog.163.com 

                                                                             -------- 2014-12-25.冷月追风

                                                                             -------- email：merafour@163.com 

1. setup  
  
    在前一篇笔记中我们把 RC_Channel一笔带过了。但其实如果你到 "ardupilot/libraries/RC_Channel/"目录下去看你会看到： 

radiolink@ubuntu:~/apm$ ls ardupilot/libraries/RC_Channel/
examples  RC_Channel_aux.cpp  RC_Channel_aux.h  RC_Channel.cpp  RC_Channel.h
radiolink@ubuntu:~/apm$

所以这里也有一个示例教你怎么使用 RC_Channel。完整路径为： "ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde"。 

#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;

RC_Channel rc_1(CH_1);
RC_Channel rc_2(CH_2);
RC_Channel rc_3(CH_3);
RC_Channel rc_4(CH_4);
RC_Channel rc_5(CH_5);
RC_Channel rc_6(CH_6);
RC_Channel rc_7(CH_7);
RC_Channel rc_8(CH_8);
RC_Channel *rc = &rc_1;

实际上 RC_Channel很容易让人误以为它是遥控器的输入，但从前一篇笔记中我们看到其实不是。这一段就是定义了八个 RC_Channel变量，也就是可以处理八个通道数据。 

void setup()
{
    hal.console->println("ArduPilot RC Channel test");
    hal.scheduler->delay(500);

    setup_radio();

    print_radio_values();

    // set type of output, symmetrical angles or a number range;
    // XXX BROKEN
    rc_1.set_angle(4500);
    rc_1.set_default_dead_zone(80);
    rc_1.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);

    // XXX BROKEN
    rc_2.set_angle(4500);
    rc_2.set_default_dead_zone(80);
    rc_2.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);

    rc_3.set_range(0,1000);
    rc_3.set_default_dead_zone(20);

    rc_4.set_angle(6000);
    rc_4.set_default_dead_zone(500);
    rc_4.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);

    rc_5.set_range(0,1000);
    rc_6.set_range(200,800);

    rc_7.set_range(0,1000);

    rc_8.set_range(0,1000);

    for (int i = 0; i < 30; i++) {
        read_radio();
    }
    rc_1.trim();
    rc_2.trim();
    rc_4.trim();
}

set_range函数跟 set_range函数前面我们已经接触过了，用来设置范围的，就不重复去看了。而 set_default_dead_zone函数根据经验是用来设置死区的。要证明这点其实也简单，我们只需跟踪代码即可。 

void RC_Channel::set_default_dead_zone(int16_t dzone)
{
    if (!_dead_zone.load()) {
        _dead_zone.set(abs(dzone));
    }
}
radiolink@ubuntu:~/apm$ grep -nr _dead_zone ./ardupilot/libraries/RC_Channel/
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:71:    rc_1.set_default_dead_zone(80);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:75:    rc_2.set_default_dead_zone(80);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:79:    rc_3.set_default_dead_zone(20);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:82:    rc_4.set_default_dead_zone(500);
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:73:    // Note: index 4 was used by the previous _dead_zone value. We
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:84:    AP_GROUPINFO("DZ",   5, RC_Channel, _dead_zone, 0),
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:115:RC_Channel::set_default_dead_zone(int16_t dzone)
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:117:    if (!_dead_zone.load()) {
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:118:        _dead_zone.set(abs(dzone));
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:241:        int16_t radio_trim_low  = radio_min + _dead_zone;
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:258:    _dead_zone.load();
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:268:    _dead_zone.save();
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:315:    returnpwm_to_angle_dz(_dead_zone);
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:358:    returnpwm_to_range_dz(_dead_zone);
./ardupilot/libraries/RC_Channel/RC_Channel.h:59:    void        set_default_dead_zone(int16_t dzone);
./ardupilot/libraries/RC_Channel/RC_Channel.h:143:    AP_Int16        _dead_zone;
radiolink@ubuntu:~/apm$

稍加分析我们就能够发现我们要去查看的其实也就是那两个 return语句。 

/*
  return an "angle in centidegrees" (normally -4500 to 4500) from
  the current radio_in value
 */
int16_t RC_Channel::pwm_to_angle()
{
    return pwm_to_angle_dz(_dead_zone);
}
/*
  convert a pulse width modulation value to a value in the configured
  range
 */

int16_t RC_Channel::pwm_to_range() 
{ 
     return pwm_to_range_dz(_dead_zone); 
} 
那么我们就列举其中一个函数来看看具体是如何使用的： 

/*
  return an "angle in centidegrees" (normally -4500 to 4500) from
  the current radio_in value using the specified dead_zone
 */
int16_t RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
{
    int16_t radio_trim_high = radio_trim + dead_zone;
    int16_t radio_trim_low  = radio_trim - dead_zone;

    // prevent div by 0
    if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
        return 0;

    if(radio_in > radio_trim_high) {
        return _reverse * ((long)_high * (long)(radio_in - radio_trim_high)) / (long)(radio_max  - radio_trim_high);
    }else if(radio_in < radio_trim_low) {
        return _reverse * ((long)_high * (long)(radio_in - radio_trim_low)) / (long)(radio_trim_low - radio_min);
    }else
        return 0;
}

稍加分析我们就会知道这是量程映射，而且其中有那么一段映射完后值始终为 0，这一段便是死区。那么最后还剩下三个函数： setup_radio、print_radio_values和 read_radio。 

void print_radio_values()
{
    for (int i=0; i<8; i++) {
         hal.console->printf("CH%u: %u|%u ",
              (unsigned)i+1, 
              (unsigned)rc[i].radio_min, 
              (unsigned)rc[i].radio_max); 
    }
}

你会发现这里打印的一个数据格式跟我们校准成功时候的信息是一致的，如： 

  

这是我们校准成功时的一个截图，那么我就有理由认为 hal.console就是跟上位机通信的那个串口。下面我们来看另外一个函数： 

void setup_radio(void)
{
    hal.console->println(" Radio Setup:");
    uint8_t i;
    
    for(i = 0; i < 100;i++){
        hal.scheduler->delay(20);
        read_radio();
    }
        
    rc_1.radio_min = rc_1.radio_in;
    rc_2.radio_min = rc_2.radio_in;
    rc_3.radio_min = rc_3.radio_in;
    rc_4.radio_min = rc_4.radio_in;
    rc_5.radio_min = rc_5.radio_in;
    rc_6.radio_min = rc_6.radio_in;
    rc_7.radio_min = rc_7.radio_in;
    rc_8.radio_min = rc_8.radio_in;

    rc_1.radio_max = rc_1.radio_in;
    rc_2.radio_max = rc_2.radio_in;
    rc_3.radio_max = rc_3.radio_in;
    rc_4.radio_max = rc_4.radio_in;
    rc_5.radio_max = rc_5.radio_in;
    rc_6.radio_max = rc_6.radio_in;
    rc_7.radio_max = rc_7.radio_in;
    rc_8.radio_max = rc_8.radio_in;

    rc_1.radio_trim = rc_1.radio_in;
    rc_2.radio_trim = rc_2.radio_in;
    rc_4.radio_trim = rc_4.radio_in;
    // 3 is not trimed
    rc_5.radio_trim = 1500;
    rc_6.radio_trim = 1500;
    rc_7.radio_trim = 1500;
    rc_8.radio_trim = 1500;
            
    hal.console->println(" Move all controls to each extreme. Hit Enter to save:");
    while(1){
        
        hal.scheduler->delay(20);
        // Filters radio input - adjust filters in the radio.pde file
        // ----------------------------------------------------------
        read_radio();

        rc_1.update_min_max();
        rc_2.update_min_max();
        rc_3.update_min_max();
        rc_4.update_min_max();
        rc_5.update_min_max();
        rc_6.update_min_max();
        rc_7.update_min_max();
        rc_8.update_min_max();
        
        if(hal.console->available() > 0) {
            hal.console->println("Radio calibrated, Showing control values:");
            break;
        }
    }
    return;
}

这个函数虽然不短，但它其实就只是获取最大值和最小值。说白了这就是用来校准遥控器的。而 read_radio函数就是用来读取遥控器数据。但我们知道，连接遥控器的是 io，所以 read_radio肯定是通过跟 io通信的方式获取数据的。但这里有个地方让我觉得很奇怪，为什么 3通道没有 " radio_trim"？结合 pwm_to_angle_dz函数我觉得比较合理的解释是 " radio_trim"是通道的中点，而三通是油门通道，不需要对中点进行处理，故而不需要中点。 

void read_radio()
{
    rc_1.set_pwm(hal.rcin->read(CH_1));
    rc_2.set_pwm(hal.rcin->read(CH_2));
    rc_3.set_pwm(hal.rcin->read(CH_3));
    rc_4.set_pwm(hal.rcin->read(CH_4));
    rc_5.set_pwm(hal.rcin->read(CH_5));
    rc_6.set_pwm(hal.rcin->read(CH_6));
    rc_7.set_pwm(hal.rcin->read(CH_7));
    rc_8.set_pwm(hal.rcin->read(CH_8));
}

前面我们在 AP_Motors_test函数中看到给 RC_Channel传值是通过 servo_out进行的，但这里我们看到了另外一种方式，即 set_pwm函数。 

// read input from APM_RC - create a control_in value
void RC_Channel::set_pwm(int16_t pwm)
{
    radio_in = pwm;

    if (_type == RC_CHANNEL_TYPE_RANGE) {
        control_in = pwm_to_range();
    } else {
        //RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
        control_in = pwm_to_angle();
    }
}

这里调用的 pwm_to_range跟 pwm_to_angle函数其实就是我们前面看到的，在这里其实就是处理死区跟映射。如果是三通最后是调用 pwm_to_range_dz函数来完成的。 

/*
  convert a pulse width modulation value to a value in the configured
  range, using the specified deadzone
 */
int16_t RC_Channel::pwm_to_range_dz(uint16_t dead_zone)
{
    int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());

    if (_reverse == -1) {
        r_in = radio_max.get() - (r_in - radio_min.get());
    }

    int16_t radio_trim_low  = radio_min + dead_zone;

    if (r_in > radio_trim_low)
        return (_low + ((long)(_high - _low) * (long)(r_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
    else if (dead_zone > 0)
        return 0;
    else
        return _low;
}

经过对比我们看到该函数只是把数据映射到 [_high - _low]， 但并没有使用中点数据。那么我们很容易想到 "hal.rcin->read"函数就是用来读遥控器数据的了。那么这个我们还得从 hal说起。 

2. hal  

    如果我们回到 HAL_PX4的构造函数： 

HAL_PX4::HAL_PX4() :
    AP_HAL::HAL(
        &uartADriver,  /* uartA */
        &uartBDriver,  /* uartB */
        &uartCDriver,  /* uartC */
        &uartDDriver,  /* uartD */
        &uartEDriver,  /* uartE */
        &i2cDriver, /* i2c */
        &spiDeviceManager, /* spi */
        &analogIn, /* analogin */
        &storageDriver, /* storage */
        &uartADriver, /* console */
        &gpioDriver, /* gpio */
        &rcinDriver,  /* rcinput */
        &rcoutDriver, /* rcoutput */
        &schedulerInstance, /* scheduler */
        &utilInstance) /* util */
{}
static PX4RCInput rcinDriver;

我们看到其中有一个 "rcinput"，它其实就是 "hal.rcin"。类定义为： 

#include <pthread.h>

class PX4::PX4RCInput : public AP_HAL::RCInput {
public:
    void init(void* machtnichts);
    bool new_input();
    uint8_t num_channels();
    uint16_t read(uint8_t ch);
    uint8_t read(uint16_t* periods, uint8_t len);

    bool set_overrides(int16_t *overrides, uint8_t len);
    bool set_override(uint8_t channel, int16_t override);
    void clear_overrides();

    void _timer_tick(void);

private:
    /* override state */
    uint16_t _override[RC_INPUT_MAX_CHANNELS];
    struct rc_input_values _rcin;
    int _rc_sub;
    uint64_t _last_read;
    bool _override_valid;
    perf_counter_t _perf_rcin;
    pthread_mutex_t rcin_mutex;
};

我们看到它其实有两个 read函数。我们也需要注意这里这个头文件： " pthread.h"，区区一个头文件为什么值得特别注意呢？因为该头文件表明这个类会去创建任务。其中 _timer_tick函数应该就是这个任务需要执行的函数。而由于该函数不是私有的，所以它可能是由外部进行调用的。 
    那么下面我们就不妨去看看这两个 read函数。 

uint16_t PX4RCInput::read(uint8_t ch) 
{
    if (ch >= RC_INPUT_MAX_CHANNELS) {
        return 0;
    }
        pthread_mutex_lock(&rcin_mutex);
    _last_read = _rcin.timestamp_last_signal;
    _override_valid = false;
    if (_override[ch]) {
            uint16_t v = _override[ch];
            pthread_mutex_unlock(&rcin_mutex);
            return v;
    }
    if (ch >= _rcin.channel_count) {
            pthread_mutex_unlock(&rcin_mutex);
            return 0;
    }
    uint16_t v = _rcin.values[ch];
        pthread_mutex_unlock(&rcin_mutex);
        return v;
}

uint8_t PX4RCInput::read(uint16_t* periods, uint8_t len) 
{
    if (len > RC_INPUT_MAX_CHANNELS) {
        len = RC_INPUT_MAX_CHANNELS;
    }
    for (uint8_t i = 0; i < len; i++){
        periods[i] = read(i);
    }
    return len;
}

最后数据来自 _override跟 _rcin.values，那这两个家伙又是什么呢？我们一个个来跟踪。 

bool PX4RCInput::set_overrides(int16_t *overrides, uint8_t len) 
{
    bool res = false;
    for (uint8_t i = 0; i < len; i++) {
        res |= set_override(i, overrides[i]);
    }
    return res;
}
bool

PX4RCInput::set_override(uint8_t channel, int16_t override) {
    if (override < 0) {
        return false; /* -1: no change. */
    }
    if (channel >= RC_INPUT_MAX_CHANNELS) {
        return false;
    }
    _override[channel] = override;
    if (override != 0) {
        _override_valid = true;
        return true;
    }
    return false;
}
radiolink@ubuntu:~/apm$ grep -nr set_overrides ./ |grep rcin 
./ardupilot/ArduCopter/GCS_Mavlink.pde:1046:        hal.rcin->set_overrides(v, 8); 
radiolink@ubuntu:~/apm$ 
所以这个就是校准的时候获取的数据。由于我们是从 io获取数据，所以我们的数据来自 _rcin.values。它是在 _timer_tick函数中把数据放进去的： 

void PX4RCInput::_timer_tick(void)
{
    perf_begin(_perf_rcin);
    bool rc_updated = false;
    if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) {
            pthread_mutex_lock(&rcin_mutex);
            orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
            pthread_mutex_unlock(&rcin_mutex);
    }
        // note, we rely on the vehicle code checking new_input() 
        // and a timeout for the last valid input to handle failsafe
    perf_end(_perf_rcin);
}

折腾了半天又是 ORB。但 ORB究竟是什么我却始终未能参透。 

int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
    int ret;

    ret = read(handle, buffer, meta->o_size);

    if (ret < 0)
        return ERROR;

    if (ret != (int)meta->o_size) {
        errno = EIO;
        return ERROR;
    }

    return OK;
}

从 orb_copy函数使用 read接口操作 _rc_sub来看 _rc_sub应该是一个文件描述符。 

void PX4RCInput::init(void* unused)
{
    _perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin");
    _rc_sub = orb_subscribe(ORB_ID(input_rc));
    if (_rc_sub == -1) {
        hal.scheduler->panic("Unable to subscribe to input_rc");        
    }
    clear_overrides();
        pthread_mutex_init(&rcin_mutex, NULL);
}
int

orb_subscribe(const struct orb_metadata *meta)
{
    return node_open(PUBSUB, meta, nullptr, false);
}
/** 
 * Common implementation for orb_advertise and orb_subscribe. 
 * 
 * Handles creation of the object and the initial publication for 
 * advertisers. 
 */ 
int node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) 
{ 
    char path[orb_maxpath]; 
    int fd, ret; 

    /* 
     * If meta is null, the object was not defined, i.e. it is not 
     * known to the system.  We can't advertise/subscribe such a thing. 
     */ 
    if (nullptr == meta) { 
        errno = ENOENT; 
        return ERROR; 
    } 

    /* 
     * Advertiser must publish an initial value. 
     */ 
    if (advertiser && (data == nullptr)) { 
        errno = EINVAL; 
        return ERROR; 
    } 

    /* 
     * Generate the path to the node and try to open it. 
     */ 
    ret = node_mkpath(path, f, meta); 

    if (ret != OK) { 
        errno = -ret; 
        return ERROR; 
    } 

    /* open the path as either the advertiser or the subscriber */ 
    fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); 

    /* we may need to advertise the node... */ 
    if (fd < 0) { 

        /* try to create the node */ 
        ret = node_advertise(meta); 

        /* on success, try the open again */ 
        if (ret == OK) 
            fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); 
    } 

    if (fd < 0) { 
        errno = EIO; 
        return ERROR; 
    } 

    /* everything has been OK, we can return the handle now */ 
    return fd; 
} 
代码跟踪到这里我们发现它直接就去 open了一个文件(应该是设备文件)，但总不可能凭空产生一个文件吧，而且就算是凭空产生了一个文件，那 read操作也不可能成功啊。而且这里面为什么要两次 open操作？ node_advertise函数有到底有什么密码呢？根据注释 " try to create the node"我觉得 node_advertise函数应该会去创建一个 node，但 node指的是设备文件吗？ 

/**
 * Advertise a node; don't consider it an error if the node has
 * already been advertised.
 *
 * @todo verify that the existing node is the same as the one
 *       we tried to advertise.
 */
int node_advertise(const struct orb_metadata *meta)
{
    int fd = -1;
    int ret = ERROR;

    /* open the control device */
    fd = open(TOPIC_MASTER_DEVICE_PATH, 0);

    if (fd < 0)
        goto out;

    /* advertise the object */
    ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta);

    /* it's OK if it already exists */
    if ((OK != ret) && (EEXIST == errno))
        ret = OK;

out:

    if (fd >= 0)
        close(fd);

    return ret;
}

真是一波未平一波又起啊，旧的文件都没搞明白，新的文件又出现了。而且貌似这里也没有去创建设备文件啊。那难道是由 TOPIC_MASTER_DEVICE_PATH这个设备来完成的吗？也只有这样能够解释得通了。详细的我们就后面再来专门讨论了。 
    那么以上就是读数据的大体过程了。 

3. loop 

    loop的代码不多，只有几行语句： 

void loop()
{
    hal.scheduler->delay(20);
    debug_rcin();
    read_radio();
    print_pwm();
}

其中 read_radio函数我们前面已经看过了， debug_rcin函数跟 print_pwm函数现在来看就简单多了。瞧： 

void debug_rcin() {
    uint16_t channels[8];
    hal.rcin->read(channels, 8);
    hal.console->printf_P(
        PSTR("rcin: %u %u %u %u %u %u %u %u "),
        channels[0],
        channels[1],
        channels[2],
        channels[3],
        channels[4],
        channels[5],
        channels[6],
        channels[7]);
}
void

print_pwm()
{
    for (int i=0; i<8; i++) {
        hal.console->printf("ch%u: %4d ", (unsigned)i+1, (int)rc[i].control_in);
    }
    hal.console->printf(" ");
} 
两个函数都是在向上位机传递数据，区别在于 debug_rcin传的是原始数据，print_pwm传的是映射后的数据。 
    关于这个测试程序就这命多了，下面我们还是赶紧来看看 ORB到底是怎么回事吧。