Example code for RCOutput: Can't get PWM Output

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 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();

Got it working: RCOut Example Code not Producing PWM Output