int fixedwing_control_main(int argc, char *argv[])
{
    /* default values for arguments */
    char *fixedwing_uart_name = "/dev/ttyS1";
    char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n";

    /* read arguments */
    bool verbose = false;

    for (int i = 1; i < argc; i++) {
        if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
            if (argc > i + 1) {
                fixedwing_uart_name = argv[i + 1];

            } else {
                printf(commandline_usage, argv[0]);
                return 0;
            }
        }
        if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
            verbose = true;
        }
    }

    /* welcome user */
    printf("[fixedwing control] started\n");

    /* Set up to publish fixed wing control messages */
    struct fixedwing_control_s control;
    int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);

    /* Subscribe to global position, attitude and rc */
    struct vehicle_global_position_s global_pos;
    int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
    struct vehicle_attitude_s att;
    int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    struct rc_channels_s rc;
    int rc_sub = orb_subscribe(ORB_ID(rc_channels));
    struct vehicle_global_position_setpoint_s global_setpoint;
    int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));

    /* Mainloop setup */
    unsigned int loopcounter = 0;
    unsigned int failcounter = 0;

    /* Control constants */
    control_outputs.mode = HIL_MODE;
    control_outputs.nav_mode = 0;

    /* Servo setup */

    int fd;
    servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];

    fd = open("/dev/pwm_servo", O_RDWR);

    if (fd < 0) {
        printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
    }

    ioctl(fd, PWM_SERVO_ARM, 0);

    int16_t buffer_rc[3];
    int16_t buffer_servo[3];
    mixer_data_t mixer_buffer;
    mixer_buffer.input  = buffer_rc;
    mixer_buffer.output = buffer_servo;

    mixer_conf_t mixers[3];

    mixers[0].source = PITCH;
    mixers[0].nr_actuators = 2;
    mixers[0].dest[0] = AIL_1;
    mixers[0].dest[1] = AIL_2;
    mixers[0].dual_rate[0] = 1;
    mixers[0].dual_rate[1] = 1;

    mixers[1].source = ROLL;
    mixers[1].nr_actuators = 2;
    mixers[1].dest[0] = AIL_1;
    mixers[1].dest[1] = AIL_2;
    mixers[1].dual_rate[0] = 1;
    mixers[1].dual_rate[1] = -1;

    mixers[2].source = THROTTLE;
    mixers[2].nr_actuators = 1;
    mixers[2].dest[0] = MOT;
    mixers[2].dual_rate[0] = 1;

    /*
     * Main control, navigation and servo routine
     */

    while(1) {
        /*
         * DATA Handling
         * Fetch current flight data
         */

        /* get position, attitude and rc inputs */
        // XXX add error checking
        orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
        orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
        orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
        orb_copy(ORB_ID(rc_channels), rc_sub, &rc);

        /* scaling factors are defined by the data from the APM Planner
         * TODO: ifdef for other parameters (HIL/Real world switch)
         */

        /* position values*/
        plane_data.lat = global_pos.lat; /// 10000000.0;
        plane_data.lon = global_pos.lon; /// 10000000.0;
        plane_data.alt = global_pos.alt; /// 1000.0f;
        plane_data.vx = global_pos.vx / 100.0f;
        plane_data.vy = global_pos.vy / 100.0f;
        plane_data.vz = global_pos.vz / 100.0f;

        /* attitude values*/
        plane_data.roll = att.roll;
        plane_data.pitch = att.pitch;
        plane_data.yaw = att.yaw;
        plane_data.rollspeed = att.rollspeed;
        plane_data.pitchspeed = att.pitchspeed;
        plane_data.yawspeed = att.yawspeed;

        /* parameter values */
        get_parameters(&plane_data, &pid_s);

        /* Attitude control part */

        if (verbose && loopcounter % 20 == 0) {
            /******************************** DEBUG OUTPUT ************************************************************/

            printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)pid_s.Kp_att, (int)pid_s.Ki_att,
                   (int)pid_s.Kd_att, (int)pid_s.intmax_att, (int)pid_s.Kp_pos, (int)pid_s.Ki_pos,
                   (int)pid_s.Kd_pos, (int)pid_s.intmax_pos, (int)plane_data.airspeed,
                   (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z,
                   (int)global_data_parameter_storage->pm.param_values[PARAM_WPLON],
                   (int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT],
                   (int)global_data_parameter_storage->pm.param_values[PARAM_WPALT],
                   global_setpoint.lat,
                   global_setpoint.lon,
                   (int)global_setpoint.altitude);

            printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
            printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
            printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*plane_data.throttle_setpoint));

            printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed(&plane_data)));

            printf("Current Position: \n %i \n %i \n %i \n", (int)plane_data.lat, (int)plane_data.lon, (int)plane_data.alt);

            printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
                   (int)(180.0f * plane_data.roll/F_M_PI), (int)(180.0f * plane_data.pitch/F_M_PI), (int)(180.0f * plane_data.yaw/F_M_PI),
                   (int)(180.0f * plane_data.rollspeed/F_M_PI), (int)(180.0f * plane_data.pitchspeed/F_M_PI), (int)(180.0f * plane_data.yawspeed)/F_M_PI);

            printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
                   (int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator),
                   (int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle));

            /************************************************************************************************************/
        }

        /*
         * Computation section
         *
         * The function calls to compute the required control values happen
         * in this section.
         */

        /* Set plane mode */
        set_plane_mode(&plane_data);

        /* Calculate the output values */
        control_outputs.roll_ailerons = calc_roll_ail(&plane_data, &pid_s);
        control_outputs.pitch_elevator = calc_pitch_elev(&plane_data, &pid_s);
        control_outputs.yaw_rudder = calc_yaw_rudder(&plane_data, &pid_s);
        control_outputs.throttle = calc_throttle(&plane_data, &pid_s);

        /*
         * Calculate rotation matrices
         */
        calc_rotation_matrix(&rmatrix_att, plane_data.roll, plane_data.pitch, plane_data.yaw);
        calc_rotation_matrix(&rmatrix_c, control_outputs.roll_ailerons, control_outputs.pitch_elevator, control_outputs.yaw_rudder);

        multiply_matrices(&rmatrix_att, &rmatrix_c, &rmatrix_b);

        calc_angles_from_rotation_matrix(&rmatrix_b, plane_data.rollb, plane_data.pitchb, plane_data.yawb);


        // TODO: fix RC input
        //if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode

        /*
         * TAKEOFF hack for HIL
         */
        if (plane_data.mode == TAKEOFF) {
            control.attitude_control_output[ROLL] = 0;
            control.attitude_control_output[PITCH] = 5000;
            control.attitude_control_output[THROTTLE] = 10000;
            //global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
        }

        if (plane_data.mode == CRUISE) {

            // TODO: substitute output with the body angles (rollb, pitchb)
            control.attitude_control_output[ROLL] = control_outputs.roll_ailerons;
            control.attitude_control_output[PITCH] = control_outputs.pitch_elevator;
            control.attitude_control_output[THROTTLE] = control_outputs.throttle;
            //control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
        }

        control.counter++;
        control.timestamp = hrt_absolute_time();
        //}

        /* Navigation part */

        /*
         * TODO: APM Planner Waypoint communication
         */
        // Get GPS Waypoint

        //	plane_data.wp_x = global_setpoint.lon;
        //	plane_data.wp_y = global_setpoint.lat;
        //	plane_data.wp_z = global_setpoint.altitude;

        /*
         * TODO: fix RC input
         */
        //if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) {	// AUTO mode
        // AUTO/HYBRID switch

        //if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) {
        plane_data.roll_setpoint = calc_roll_setpoint(35.0f, &plane_data);
        plane_data.pitch_setpoint = calc_pitch_setpoint(35.0f, &plane_data);
        plane_data.throttle_setpoint = calc_throttle_setpoint(&plane_data);

//			} else {
//				plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200;
//				plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200;
//				plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200;
//			}

        //control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);

//		} else {
//			control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000;
//			control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000;
//			control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000;
        // since we don't have a yaw rudder
        //control.attitude_control_output[YAW] = rc.chan[rc.function[YAW]].scale/10000;

        control.counter++;
        control.timestamp = hrt_absolute_time();
        //}

        /* publish the control data */

        orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);

        /* Servo part */

        buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]);
        buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]);
        buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]);

        //mix_and_link(mixers, 3, 2, &mixer_buffer);

        // Scaling and saturation of servo outputs happens here

        data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
                      + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM];

        if (data[AIL_1] > SERVO_MAX)
            data[AIL_1] = SERVO_MAX;

        if (data[AIL_1] < SERVO_MIN)
            data[AIL_1] = SERVO_MIN;

        data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
                      + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM];

        if (data[AIL_2] > SERVO_MAX)
            data[AIL_2] = SERVO_MAX;

        if (data[AIL_2] < SERVO_MIN)
            data[AIL_2] = SERVO_MIN;

        data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
                    + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM];

        if (data[MOT] > SERVO_MAX)
            data[MOT] = SERVO_MAX;

        if (data[MOT] < SERVO_MIN)
            data[MOT] = SERVO_MIN;

        int result = write(fd, &data, sizeof(data));

        if (result != sizeof(data)) {
            if (failcounter < 10 || failcounter % 20 == 0) {
                printf("[fixedwing_control] failed writing servo outputs\n");
            }
            failcounter++;
        }

        loopcounter++;

        /* 20Hz loop*/
        usleep(50000);
    }

    return 0;
}
/*******************************************************************************
 * *nav_loop()
 *
 * Navigation control loop of the fixedwing controller
 * This loop calculates the roll,pitch and throttle setpoints, which are needed
 * to attain the desired heading, altitude and velocity.
 *
 * Input: none
 *
 * Output: none
 *
 ******************************************************************************/
static void *nav_loop(void *arg)
{
	// Set thread name
	prctl(1, "fixedwing_control nav", getpid());

	/* Set up to publish fixed wing control messages */
	struct fixedwing_control_s control;
	int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);

	/* Subscribe to global position, attitude and rc */
	struct vehicle_global_position_s global_pos;
	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	struct vehicle_attitude_s att;
	int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	struct rc_channels_s rc;
	int rc_sub = orb_subscribe(ORB_ID(rc_channels));

	while (1) {

		/* get position, attitude and rc inputs */
		// XXX add error checking
		orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
		orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);

		/* scaling factors are defined by the data from the APM Planner
		 * TODO: ifdef for other parameters (HIL/Real world switch)
		 */
		plane_data.lat = global_pos.lat / 10000000;
		plane_data.lon = global_pos.lon / 10000000;
		plane_data.alt = global_pos.alt / 1000;
		plane_data.vx = global_pos.vx / 100;
		plane_data.vy = global_pos.vy / 100;
		plane_data.vz = global_pos.vz / 100;

		plane_data.roll = att.roll;
		plane_data.pitch = att.pitch;
		plane_data.yaw = att.yaw;
		plane_data.rollspeed = att.rollspeed;
		plane_data.pitchspeed = att.pitchspeed;
		plane_data.yawspeed = att.yawspeed;

		// Get GPS Waypoint

//		if(global_data_wait(&global_data_position_setpoint->access_conf) == 0)
//		{
//			plane_data.wp_x = global_data_position_setpoint->x;
//			plane_data.wp_y = global_data_position_setpoint->y;
//			plane_data.wp_z = global_data_position_setpoint->z;
//		}
//		global_data_unlock(&global_data_position_setpoint->access_conf);

		if (global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale < MANUAL) {	// AUTO mode
			// AUTO/HYBRID switch

			if (global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale < AUTO) {
				plane_data.roll_setpoint = calc_roll_setpoint();
				plane_data.pitch_setpoint = calc_pitch_setpoint();
				plane_data.throttle_setpoint = calc_throttle_setpoint();

			} else {
				plane_data.roll_setpoint = global_data_rc_channels->chan[global_data_rc_channels->function[ROLL]].scale / 200;
				plane_data.pitch_setpoint = global_data_rc_channels->chan[global_data_rc_channels->function[PITCH]].scale / 200;
				plane_data.throttle_setpoint = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].scale / 200;
			}

			//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);

			// 10 Hz loop
			usleep(100000);

		} else {
			control->attitude_control_output[ROLL] = global_data_rc_channels->chan[global_data_rc_channels->function[ROLL]].scale;
			control->attitude_control_output[PITCH] = global_data_rc_channels->chan[global_data_rc_channels->function[PITCH]].scale;
			control->attitude_control_output[THROTTLE] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].scale;
			// since we don't have a yaw rudder
			//global_data_fixedwing_control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale;

			control->counter++;
			control->timestamp = hrt_absolute_time();

#error Either publish here or above, not at two locations
			orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);

			usleep(100000);
		}
	}

	return NULL;
}