// init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration void Copter::init_rc_out() { motors.set_update_rate(g.rc_speed); motors.set_frame_orientation(g.frame_orientation); motors.set_loop_rate(scheduler.get_loop_rate_hz()); motors.Init(); // motor initialisation #if FRAME_CONFIG != HELI_FRAME motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); #endif for(uint8_t i = 0; i < 5; i++) { delay(20); read_radio(); } // we want the input to be scaled correctly channel_throttle->set_range_out(0,1000); // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); // check if we should enter esc calibration mode esc_calibration_startup_check(); // enable output to motors pre_arm_rc_checks(); if (ap.pre_arm_rc_check) { enable_motor_output(); } // refresh auxiliary channel to function map RC_Channel_aux::update_aux_servo_function(); }
// init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration void Copter::init_rc_out() { motors->set_loop_rate(scheduler.get_loop_rate_hz()); motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get()); // enable aux servos to cope with multiple output channels per motor SRV_Channels::enable_aux_servos(); // update rate must be set after motors->init() to allow for motor mapping motors->set_update_rate(g.rc_speed); #if FRAME_CONFIG != HELI_FRAME motors->set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); #else // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); #endif // refresh auxiliary channel to function map SRV_Channels::update_aux_servo_function(); #if FRAME_CONFIG != HELI_FRAME /* setup a default safety ignore mask, so that servo gimbals can be active while safety is on */ uint16_t safety_ignore_mask = (~copter.motors->get_motor_mask()) & 0x3FFF; BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask); #endif // check if we should enter esc calibration mode esc_calibration_startup_check(); }