Dual swashplate coax Helicopter

I am working on a coaxcopter project with two gimbals to control the pitch and roll by tilting each motor. The copter is unable to balance itself with the default firmware, tried different servo configurations, verified the CG, tried different 3D printed designs for the gimbals and replaced all the component and tested it on a quad as well for ensuring that they work properly and the log shows no errors.

I have been trying to reverse the servos for only when the copter tries to balance but I wasn’t able to do that without making any changes in the code as the servos get reversed for the RCINPUT as well. Below is an illustration of what I mean.

Figure 1 shows how the propellers should tilt while balancing.
Figure 2 shows how the propellers should tilt while moving forward (ideally).

I made some changes in the AP_MotorsCoax,cpp file as shown below, with these changes the drone is able to balance and responds correctly in stabilize mode, but i am not sure how can I achieve the same for Loiter and other AUTO modes.

 float   roll_thrust;                // roll thrust input value, +/- 1.0
    float   pitch_thrust;               // pitch thrust input value, +/- 1.0
    float   roll_thrust_1;
    float   pitch_thrust_1;
    float   yaw_thrust;                 // yaw thrust input value, +/- 1.0
    float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
    float   throttle_avg_max;           // throttle thrust average maximum value, 0.0 - 1.0
    float   thrust_min_rpy;             // the minimum throttle setting that will not limit the roll and pitch output
    float   thr_adj;                    // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
    float   thrust_out;                 //
    float   rp_scale = 1.0f;            // this is used to scale the roll, pitch and yaw to fit within the motor limits
    float   actuator_allowed = 0.0f;    // amount of yaw we can fit in
    float roll_radio_scaled = _roll_radio_passthrough * 0.25f;
    float pitch_radio_scaled = _pitch_radio_passthrough * 0.25f;
    //GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "radio_passthrough: %f", _roll_radio_passthrough);

    float _roll_in_scaled = _roll_in - roll_radio_scaled;
    float _pitch_in_scaled = _pitch_in - pitch_radio_scaled;

    // apply voltage and air pressure compensation
    const float compensation_gain = get_compensation_gain();
    roll_thrust = ((_roll_in_scaled + roll_radio_scaled) + _roll_in_ff) * compensation_gain;
    pitch_thrust = ((_pitch_in_scaled + pitch_radio_scaled) + _pitch_in_ff) * compensation_gain;
    roll_thrust_1 = ((_roll_in_scaled - roll_radio_scaled) + _roll_in_ff) * compensation_gain;
    pitch_thrust_1 = ((_pitch_in_scaled - pitch_radio_scaled) + _pitch_in_ff) * compensation_gain;
    yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
    throttle_thrust = get_throttle() * compensation_gain;
    throttle_avg_max = _throttle_avg_max * compensation_gain;
_actuator_out[0] = roll_thrust / thrust_out_actuator;
    _actuator_out[1] = pitch_thrust / thrust_out_actuator;
    _actuator_out[2] = roll_thrust_1 / thrust_out_actuator;
    _actuator_out[3] = pitch_thrust_1 / thrust_out_actuator;

    if (fabsf(_actuator_out[0]) > 1.0f) {
        limit.roll = true;
        _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
    if (fabsf(_actuator_out[1]) > 1.0f) {
        limit.pitch = true;
        _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
    if (fabsf(_actuator_out[2]) > 1.0f) {
        limit.roll = true;
        _actuator_out[2] = constrain_float(_actuator_out[2], -1.0f, 1.0f);
    if (fabsf(_actuator_out[3]) > 1.0f) {
        limit.pitch = true;
        _actuator_out[3] = constrain_float(_actuator_out[3], -1.0f, 1.0f);

With these changes the copter works in stabilize mode.

I am using Pixhawk 6C, also tried with Pixhawk 6X and Durandal with Arducopter 4.3.

If anybody can point out whats wrong and how can I achieve the stable flight in loiter or make right changes to the code, this was my first attempt to work with ardupilot codes. Any help is appreciated or if you can connect me to right person it would a great help.

I would like to know if there are functions that returns the values for movement of the drone separately and the output values for the orientations separately which i can make use of in the above code instead of using radio_passthrough.

Any suggestions on how can I achieve the desired output as mentioned above in more simpler way that works in all other flight modes?