RCOut Example Code not Producing PWM Output

ArduCopter
1

I’ve been trying to get the code in AP_HAL/examples/RCOut.cpp to output a PWM command to a servo on any channel on my Pixhawk4 (px4-v2) and failed so far. The servo moves in one direction by some amount but doesn’t move back as should happen in the example code (it moves only when I plug in the battery, so it seems it’s just a startup transient).

I believe my issue is with arming/turning off the output safety flags for the board, so I’ve modified the code slightly in an attempt to do this. I’m not using a physical safety switch and want to be able to enable servo outputs.

I’ve powered the servo rail using a BEC, and with Copter 3.6 loaded I am able to get the servo to respond in Mission Planner using the servo toggle functions (even without arming the board using a switch or turning off the arm checks in config/tuning).

What am I missing? So far while learning the code, simply running the examples without change has worked,but not this time.

Here’s my modified code (note the basic example doesn’t work):

/*
simple test of RC output interface
*/

#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/RCOutput.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Param/AP_Param.h>

//#include <AP_HAL_PX4/RCOutput.h>
#include <AP_HAL_PX4/RCOutput.h>

///////////////////////////////////////////////////////////////////////////////////////
// Just so that it’s completely clear…
#define ENABLED 1
#define DISABLED 0

// this avoids a very common config error
#define ENABLE ENABLED
#define DISABLE DISABLED
///////////////////////////////////////////////////////////////////////////////////////

AP_BoardConfig BoardConfig;

void setup();
void loop();

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

void setup (void)
{

hal.console->printf("Starting AP_HAL::RCOutput test\n");

for (uint8_t i = 0; i< 14; i++) {
    hal.rcout->enable_ch(i);
    hal.console->printf("Enable Channel %u \n", i);
}


hal.rcout->force_safety_off();
//Change parameters for board
float SET_PWM_PARAMETER = 14;
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, "PWM_COUNT", SET_PWM_PARAMETER);
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, "SAFETYENABLE", DISABLE);
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, "SAFETYOPTION", 0);// 0 1 2
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, "SAFETY_MASK", 8191);

}

static uint16_t pwm = 1500;
static int8_t delta = 1;

void loop (void)
{

for (uint8_t i=0; i < 14; i++) {

    hal.rcout->enable_ch(i); //ADDED
    hal.console->printf("Enabled Channel %u \n", i);
    hal.rcout->write(i, pwm); //ONLY CHANNEL 2; (uint8_t)2

    pwm += delta;

    hal.console->printf("Driving Channel %u at %u Hz and PWM %u, last sent %u, read output value: %u\n",
            i,
            hal.rcout->get_freq(i),
            pwm,
            hal.rcout->read_last_sent(i),
            hal.rcout->read(i));

    hal.console->printf("Enabled PWMs: %u, Safety Mask: %u, IO Enable: %u \n",
            BoardConfig.get_pwm_count(),
            BoardConfig.get_safety_mask(),
            BoardConfig.io_enabled());

    if (delta > 0 && pwm >= 1900) {  //ORIGINALLY 2000
        delta = -1;
        hal.console->printf("Reversing Channel %u at %u Hz and PWM %u\n", i, hal.rcout->get_freq(i),pwm);
        hal.scheduler->delay(1000); //was 5
    } else if (delta < 0 && pwm <= 1100) { //ORIGINALLY 1000
        delta = 1;
        hal.console->printf("Normalizing Channel %u at %u Hz and PWM %u\n", i, hal.rcout->get_freq(i),pwm);
        hal.scheduler->delay(1000); //was 5
    }
}
hal.scheduler->delay(100); //was 5

}

AP_HAL_MAIN();

2

Got it working. I believe it was an issue with initializing the board. Once I copied the <AP_IOMCU/AP_IOMCU.h> from the working RCIn example, and initialized the board in setup() with BoardConfig.init(), everything seemed to work.

/*
simple test of RC output interface
*/

#include <AP_HAL/AP_HAL.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Param/AP_Param.h>
#include <AP_Common/AP_Common.h>
// we need a boardconfig created so that the io processor’s enable
// parameter is available
#if HAL_WITH_IO_MCU || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_IOMCU/AP_IOMCU.h>
AP_BoardConfig BoardConfig;
#endif

///////////////////////////////////////////////////////////////////////////////////////
// Just so that it’s completely clear…
#define ENABLED 1
#define DISABLED 0

// this avoids a very common config error
#define ENABLE ENABLED
#define DISABLE DISABLED
///////////////////////////////////////////////////////////////////////////////////////

#define ESC_HZ 490
#define SERVO_HZ 50
///////////////////////////////////////////////////////////////////////////////////////

void setup();
void loop();

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

void setup (void)
{

hal.console->printf("Starting AP_HAL::RCOutput test\n");

#if HAL_WITH_IO_MCU || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
BoardConfig.init();
#endif

for (uint8_t i = 0; i< 14; i++) {
    hal.rcout->enable_ch(i);
    hal.console->printf("Enable Channel %u \n", i);
}

/////////////

#if HAL_WITH_IO_MCU || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
//Change parameters for board
float SET_PWM_PARAMETER = 14;
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, “PWM_COUNT”, SET_PWM_PARAMETER);
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, “SAFETYENABLE”, DISABLE);
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, “SAFETYOPTION”, 0);// 0 1 2
AP_Param::set_object_value(&BoardConfig, BoardConfig.var_info, “SAFETY_MASK”, 8191);
#endif

hal.rcout->set_freq(0xFF, SERVO_HZ); //// set output frequency

}

static uint16_t pwm = 1500;
static int8_t delta = 1;

void loop (void)
{
hal.rcout->force_safety_off();

// hal.rcout->set_safety_pwm(0xF, pwm_failsafe); //

// hal.rcout->cork();
for (uint8_t i=0; i < 14; i++) {

    hal.rcout->enable_ch(i); //ADDED
    hal.console->printf("Enabled Channel %u \n", i);
    hal.rcout->write(i, pwm); //ONLY CHANNEL 2; (uint8_t)2

    pwm += delta;

    hal.console->printf("Driving Channel %u at %u Hz and PWM %u, last sent %u, read output value: %u\n",
            i,
            hal.rcout->get_freq(i),
            pwm,
            hal.rcout->read_last_sent(i),
            hal.rcout->read(i));

    hal.console->printf("Enabled PWMs: %u, Safety Mask: %u, IO Enable: %u \n",
            BoardConfig.get_pwm_count(),
            BoardConfig.get_safety_mask(),
            BoardConfig.io_enabled());

    if (delta > 0 && pwm >= 1400) {  //ORIGINALLY 2000
        delta = -1;
        hal.console->printf("Reversing Channel %u at %u Hz and PWM %u\n", i, hal.rcout->get_freq(i),pwm);
        hal.scheduler->delay(5); //was 5
    } else if (delta < 0 && pwm <= 1100) { //ORIGINALLY 1000
        delta = 1;
        hal.console->printf("Normalizing Channel %u at %u Hz and PWM %u\n", i, hal.rcout->get_freq(i),pwm);
        hal.scheduler->delay(5); //was 5
    }

}

// hal.rcout->push();

if(hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_ARMED){
    hal.console->printf("SAFETY_ARMED \n");
}
if(hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED){
    hal.console->printf("SAFETY_DISARMED \n");
}
if(hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_NONE){
    hal.console->printf("SAFETY_NONE \n");
}

hal.console->printf("ARMED?: %d",hal.util->get_soft_armed());

hal.scheduler->delay(5); //was 5

}

AP_HAL_MAIN();