int
set_hil_on_off(bool hil_enabled)
{
	int ret = OK;

	/* Enable HIL */
	if (hil_enabled && !mavlink_hil_enabled) {

		/* Advertise topics */
		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);

		/* sensore level hil */
		pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
		pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);

		mavlink_hil_enabled = true;

		/* ramp up some HIL-related subscriptions */
		unsigned hil_rate_interval;

		if (baudrate < 19200) {
			/* 10 Hz */
			hil_rate_interval = 100;

		} else if (baudrate < 38400) {
			/* 10 Hz */
			hil_rate_interval = 100;

		} else if (baudrate < 115200) {
			/* 20 Hz */
			hil_rate_interval = 50;

		} else {
			/* 200 Hz */
			hil_rate_interval = 5;
		}

		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
	}

	if (!hil_enabled && mavlink_hil_enabled) {
		mavlink_hil_enabled = false;
		orb_set_interval(mavlink_subs.spa_sub, 200);

	} else {
		ret = ERROR;
	}

	return ret;
}
Exemple #2
0
int
set_hil_on_off(bool hil_enabled)
{
	int ret = OK;

	/* Enable HIL */
	if (hil_enabled && !mavlink_hil_enabled) {

		//printf("\n HIL ON \n");

		/* Advertise topics */
		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);

		printf("\n pub_hil_attitude :%i\n", pub_hil_attitude);
		printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos);

		mavlink_hil_enabled = true;

		/* ramp up some HIL-related subscriptions */
		unsigned hil_rate_interval;

		if (baudrate < 19200) {
			/* 10 Hz */
			hil_rate_interval = 100;
		} else if (baudrate < 38400) {
			/* 10 Hz */
			hil_rate_interval = 100;
		} else if (baudrate < 115200) {
			/* 20 Hz */
			hil_rate_interval = 50;
		} else if (baudrate < 460800) {
			/* 50 Hz */
			hil_rate_interval = 20;
		} else {
			/* 100 Hz */
			hil_rate_interval = 10;
		}

		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
	}

	if (!hil_enabled && mavlink_hil_enabled) {
		mavlink_hil_enabled = false;
		orb_set_interval(mavlink_subs.spa_sub, 200);

	} else {
		ret = ERROR;
	}

	return ret;
}
/**
 * Main execution thread
 */
int unibo_ECF_standard_thread_main(int argc, char *argv[])
{

    warnx("[unibo_ECF_standard] starting\n");

    thread_running = true;

    warnx("Hello Sky!\n");
    model = ECF_stand_q(); //Init model!

    /* subscribe sensor measurements from sensor_combined  */
    struct sensor_combined_s sens_mes;
    int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
    /* set data to 1Hz */
    orb_set_interval(sensor_sub_fd, 3); //1000 = 1Hz (ms)


    /* advertise attitude topic */
    struct vehicle_attitude_s ECF_out;
    memset(&ECF_out, 0, sizeof(ECF_out));
    int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &ECF_out);

    /* one could wait for multiple topics with this technique, just using one here */
    struct pollfd fds[] = {
        { .fd = sensor_sub_fd,   .events = POLLIN },
        /* there could be more file descriptors here, in the form like:
         * { .fd = other_sub_fd,   .events = POLLIN },
         */
    };
void simpleapp_task() {
	printf("Initializing /dev/ttyS2...\n");

	int uartfd = open("/dev/ttyS2", O_RDWR | O_NOCTTY); // TELEM2 port
	assert(uartfd >= 0);
	// manage terminal settings
	int termios_state_ttyS2;
	struct termios existing_config_ttyS2;
	// get existing terminal config and store it.
	assert((termios_state_ttyS2 = tcgetattr(uartfd, &existing_config_ttyS2)) >= 0);
	struct termios config_ttyS2;
	// duplicate into the new config
	tcgetattr(uartfd, &config_ttyS2);
	// memcpy(config_ttyS2, existing_config_ttyS2);

	// clear ONLCR flag
	config_ttyS2.c_oflag &= ~ONLCR;
	// set baud rate
	assert(cfsetispeed(&config_ttyS2, B921600) >= 0 || cfsetospeed(&config_ttyS2, B921600) >= 0);
	// go ahead and set the config i am setting up
	assert((termios_state_ttyS2 = tcsetattr(uartfd, TCSANOW, &config_ttyS2)) >= 0);

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	orb_set_interval(sensor_sub_fd, 2);

	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		{ .fd = other_sub_fd,   .events = POLLIN },
		*/
	};
Exemple #5
0
/**
 * Main loop function
 */
void
PCA9685::i2cpwm()
{
	if (_mode == IOX_MODE_TEST_OUT) {
		setPin(0, PCA9685_PWMCENTER);
		_should_run = true;

	} else if (_mode == IOX_MODE_OFF) {
		_should_run = false;

	} else {
		if (!_mode_on_initialized) {
			/* Subscribe to actuator control 2 (payload group for gimbal) */
			_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
			/* set the uorb update interval lower than the driver pwm interval */
			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);

			_mode_on_initialized = true;
		}

		/* Read the servo setpoints from the actuator control topics (gimbal) */
		bool updated;
		orb_check(_actuator_controls_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);

			for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
				/* Scale the controls to PWM, first multiply by pi to get rad,
				 * the control[i] values are on the range -1 ... 1 */
				uint16_t new_value = PCA9685_PWMCENTER +
						     (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
				DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
					     (double)_actuator_controls.control[i]);

				if (new_value != _current_values[i] &&
				    isfinite(new_value) &&
				    new_value >= PCA9685_PWMMIN &&
				    new_value <= PCA9685_PWMMAX) {
					/* This value was updated, send the command to adjust the PWM value */
					setPin(i, new_value);
					_current_values[i] = new_value;
				}
			}
		}

		_should_run = true;
	}

	// check if any activity remains, else stop
	if (!_should_run) {
		_running = false;
		return;
	}

	// re-queue ourselves to run again later
	_running = true;
	work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}
Subscription<T>::Subscription(
	List<SubscriptionBase *> * list,
	const struct orb_metadata *meta, unsigned interval) :
	T(), // initialize data structure to zero
	SubscriptionBase(list, meta) {
	setHandle(orb_subscribe(getMeta()));
	orb_set_interval(getHandle(), interval);
}
int att_read_simple_main(int argc, char *argv[]) {
    PX4_INFO("Hello Sky!");
    
    int att = orb_subscribe(ORB_ID(vehicle_attitude));
    orb_set_interval(att, 1000);

    px4_pollfd_struct_t fds[] = {
            { .fd = att,   .events = POLLIN },
    };
// thread principale con loop
int unibo_motor_output_thread_main(int argc, char *argv[])
{
	int count=0;
	warnx("[unibo_motor_output] starting\n");
	unibomo_thread_running = true;

	// subscribe a ORB
	int motor_output_fd = orb_subscribe(ORB_ID(motor_output));
	orb_set_interval(motor_output_fd, 3);

	struct pollfd fds[] = { { .fd = motor_output_fd, .events = POLLIN } };
Exemple #9
0
int test_main(int argc, char *argv[])
{
	printf("Hello There!\n");

    // TESTING for ADC input
    int adc_prox_sub_fd = orb_subscribe(ORB_ID(adc_prox));
    orb_set_interval(adc_prox_sub_fd, 100);

    struct pollfd fds[] = {
        {.fd=adc_prox_sub_fd,   .events =POLLIN},
    };
int InputMavlinkCmdMount::initialize()
{
	if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) {
		return -errno;
	}

	// rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode,
	// it will publish vehicle_command's as well, causing the input poll() in here to return
	// immediately, which in turn will cause an output update and thus a busy loop.
	orb_set_interval(_vehicle_command_sub, 10);

	return 0;
}
int uuv_example_app_main(int argc, char *argv[])
{
	PX4_INFO("auv_hippocampus_example_app has been started!");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	/* limit the update rate to 5 Hz */
	orb_set_interval(sensor_sub_fd, 200);

	/* subscribe to control_state topic */
	int vehicle_attitude_sub_fd = orb_subscribe(ORB_ID(vehicle_attitude));
	/* limit the update rate to 5 Hz */
	orb_set_interval(vehicle_attitude_sub_fd, 200);

	/* advertise to actuator_control topic */
	struct actuator_controls_s act;
	memset(&act, 0, sizeof(act));
	orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_controls_0), &act);

	/* one could wait for multiple topics with this technique, just using one here */
	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		{ .fd = vehicle_attitude_sub_fd,   .events = POLLIN },
Exemple #12
0
void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval)
{
	if (orb_exists(id, topic_instance) != 0) {
		printf("never published\n");
		return;
	}

	int sub = orb_subscribe_multi(id, topic_instance);
	orb_set_interval(sub, topic_interval);

	bool updated = false;
	unsigned i = 0;
	hrt_abstime start_time = hrt_absolute_time();

	while (i < num_msgs) {
		orb_check(sub, &updated);

		if (i == 0) {
			updated = true;

		} else {
			usleep(500);
		}

		if (updated) {
			start_time = hrt_absolute_time();
			i++;

			printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);

			T container;

			if (orb_copy(id, sub, &container) == PX4_OK) {
				print_message(container);

			} else {
				PX4_ERR("orb_copy failed");
			}

		} else {
			if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
				printf("Waited for 2 seconds without a message. Giving up.\n");
				break;
			}
		}
	}

	orb_unsubscribe(sub);
}
Exemple #13
0
int helloSky_main(int argc, char *argv[])
{
	printf("Hello Sky!\n");
 
	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	// set up the discretization time 
	orb_set_interval(sensor_sub_fd, 1000);
 
	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};
Exemple #14
0
SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta,
				   unsigned interval, unsigned instance) :
	_meta(meta),
	_instance(instance),
	_handle()
{
	if (_instance > 0) {
		_handle =  orb_subscribe_multi(
				   getMeta(), instance);

	} else {
		_handle =  orb_subscribe(getMeta());
	}

	if (_handle < 0) { warnx("sub failed"); }

	orb_set_interval(getHandle(), interval);
}
int px4_simple_app_main(int argc, char *argv[])
{
	printf("Hello Sky!\n");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	orb_set_interval(sensor_sub_fd, 1000);

	/* advertise attitude topic */
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};
Exemple #16
0
int wastegate_ctrl_main(int argc, char *argv[])
{
	PX4_INFO("Starting Fireblade Wastegate Control Application");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_wastegate));
	orb_set_interval(sensor_sub_fd, 1000);

	/* advertise attitude topic */
	struct actuator_wastegate_s act;
	memset(&act, 0, sizeof(act));
	orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_wastegate), &act);

	/* one could wait for multiple topics with this technique, just using one here */
	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};
static int rpgRateControllerThreadMain(int argc, char *argv[])
{
  struct imu_msg_s imu_msg;
  memset(&imu_msg, 0, sizeof(imu_msg));
  struct offboard_control_setpoint_s offboard_sp;
  memset(&offboard_sp, 0, sizeof(offboard_sp));
  struct offboard_control_setpoint_s laird_sp;
  memset(&laird_sp, 0, sizeof(laird_sp));
  struct torques_and_thrust_s desired_torques_and_thrust;
  memset(&desired_torques_and_thrust, 0, sizeof(desired_torques_and_thrust));

  int param_sub = orb_subscribe(ORB_ID(parameter_update));
  int offboard_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
  int laird_sub = orb_subscribe(ORB_ID(laird_control_setpoint));
  int imu_sub = orb_subscribe(ORB_ID(imu_msg));

  orb_advert_t rotor_thrusts_pub = orb_advertise(ORB_ID(torques_and_thrust), &desired_torques_and_thrust);

  // Limit this loop frequency to 200Hz
  orb_set_interval(imu_sub, 5);

  struct pollfd fds[2] = { {.fd = imu_sub, .events = POLLIN}, {.fd = param_sub, .events = POLLIN}};
int px4_simple_main(int argc, char *argv[])
{
    PX4_INFO("Hello Sky!");

    //ORB_DECLARE(distance_around);
    /* subscribe to sensor_combined topic */
    int sonardata_sub_fd = orb_subscribe(ORB_ID(SonarGroupDistance));
    orb_set_interval(sonardata_sub_fd, 1000);

    /* advertise attitude topic */
    struct SonarGroupDistance_s sonardata;

    memset(&sonardata, 0, sizeof(sonardata));

    /* one could wait for multiple topics with this technique, just using one here */
    px4_pollfd_struct_t fds[] = {
        { .fd = sonardata_sub_fd,   .events = POLLIN },
        //      { .fd = distancearound_sub_fd,   .events = POLLIN },
        /* there could be more file descriptors here, in the form like:
                                 * { .fd = other_sub_fd,   .events = POLLIN },
                                 */
    };
/**
 * Main execution thread
 */
int unibo_trajectory_ref_thread_main(int argc, char *argv[])
//int simple_test_app_main(int argc, char *argv[])
{
	warnx("[unibo_trajectory_ref] starting\n");

	thread_running = true;

	model = TRAJECTORY_GENERATOR_APP(); //Init model!

	/* subscribe to attitude topic */
	int vehicle_attitude_fd = orb_subscribe(ORB_ID(vehicle_attitude));

	/* subscribe to joystick topic */
	int joystick_fd = orb_subscribe(ORB_ID(unibo_joystick));
	orb_set_interval(joystick_fd, 10); //1000 = 1Hz (ms)   100 HZ!!

	/* subscribe to position topic */
	int local_position_fd = orb_subscribe(ORB_ID(vehicle_local_position));

	/* subscribe to parameters topic */
	int unibo_parameters_fd = orb_subscribe(ORB_ID(unibo_parameters));


	/* advertise reference topic */
	struct unibo_reference_s reference;
	memset(&reference, 0, sizeof(reference));
	int reference_pub_fd = orb_advertise(ORB_ID(unibo_reference), &reference);

	/* advertise local_pos_setpoint topic */
	struct vehicle_local_position_setpoint_s setpoint;
	memset(&setpoint, 0, sizeof(setpoint));
	int setpoint_pub_fd = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &setpoint);

	/* advertise local_pos_setpoint topic */
	struct position_setpoint_triplet_s setpoint_triplet;
	memset(&setpoint_triplet, 0, sizeof(setpoint_triplet));
	int setpoint_triplet_pub_fd = orb_advertise(ORB_ID(position_setpoint_triplet), &setpoint_triplet);

	/* one could wait for multiple topics with this technique, just using one here */
//	struct pollfd fds[] = {
//		{ .fd = joystick_fd,   .events = POLLIN },                                //CAMBIARE in joystick
//		/* there could be more file descriptors here, in the form like:
//		 * { .fd = other_sub_fd,   .events = POLLIN },
//		 */
//	};
//
//	int error_counter = 0;


	/*
	 * |-----------------------------------------------------|
	 * |                  INIT VARIABLES!                    |
	 * |-----------------------------------------------------|
	 */



	// inizializzazione middle-layer
	struct vehicle_attitude_s ahrs;
	struct unibo_joystick_s joystick;
	struct vehicle_local_position_s position;
	struct unibo_parameters_s param;

	static uint64_t time_pre = 0;
	static uint64_t nowT = 0;
	float deltaT;


	TRAJ_start();
	TRAJ_control();



	/* Bool for topics update */
	bool updated;

	float yawoffset = 0;   //offset between onboard yaw and optitrack yaw


	/*
	 * |-----------------------------------------------------|
	 * |                MAIN THREAD LOOP!                    |
	 * |-----------------------------------------------------|
	 */


	while (!thread_should_exit) {
		//TECNICAMENTE SIAMO GIA' A 50HZ, freq del topic di joystick (default 50HZ)

		nowT = hrt_absolute_time();
		deltaT = (nowT-time_pre)/1000.0f;
		if (deltaT>=20){              //50 Hz
			time_pre = nowT;
			//warnx("Trajectory APP: deltaT = %.2f",deltaT);    //milliseconds deltaT, should be 20

			/* copy raw JOYSTICK data into local buffer */
			orb_check(joystick_fd, &updated);
			if (updated){
				orb_copy(ORB_ID(unibo_joystick), joystick_fd, &joystick);
				for  (int i = 0; i<4;i++){
					TRAJECTORY_GENERATOR_APP_U.JOYSTICK[i] = joystick.axis[i];        //position and yaw command
				}
				TRAJECTORY_GENERATOR_APP_U.JOYSTICK[4] = joystick.buttons;          //button
			}


			//warnx("Joystick packet received: CH1: %d - CH2: %d - CH3: %d - CH4: %d BUTTON: %d",joystick.axis[0],joystick.axis[1],joystick.axis[2],joystick.axis[3],joystick.buttons);

//			orb_check(unibo_parameters_fd, &updated);   //TODO
//			if (updated){
//				orb_copy(ORB_ID(unibo_parameters), unibo_parameters_fd, &param);
//				yawoffset = param.in24;
//			}

			/* copy attitude into local buffer */
			orb_check(vehicle_attitude_fd, &updated);
			if (updated){
				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_fd, &ahrs);
				TRAJECTORY_GENERATOR_APP_U.PSI = ahrs.yaw;
			}

			/* copy position data into local buffer */                 //TODO change to local position for compatibility
			orb_check(local_position_fd, &updated);
			if (updated){
				orb_copy(ORB_ID(vehicle_local_position), local_position_fd, &position);
				TRAJECTORY_GENERATOR_APP_U.Position [0] = position.x;
				TRAJECTORY_GENERATOR_APP_U.Position [1] = position.y;
				TRAJECTORY_GENERATOR_APP_U.Position [2] = position.z;
			}

			TRAJECTORY_GENERATOR_APP_U.BODY_INERT = true;      //position references in body frame (take into account actual yaw)
			TRAJECTORY_GENERATOR_APP_U.TSTAMP = 0;             //TODO add timestamp



			/*
			 * |-----------------------------------------------------|
			 * |                EXECUTION LOOP!                      |
			 * |-----------------------------------------------------|
			 */



			// ----------- EXECUTION -----------
			TRAJ_control();



			/* Fill References structure and write into topic*/
			reference.p_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[0];       //Position
			reference.p_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[1];
			reference.p_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[2];
			reference.dp_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[3];      //Velocity
			reference.dp_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[4];
			reference.dp_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[5];
			reference.ddp_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[6];     //Acceleration
			reference.ddp_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[7];
			reference.ddp_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[8];
//			reference.d3p_x = 0;     //Jerk
//			reference.d3p_y = 0;
//			reference.d3p_z = 0;
//			reference.d4p_x = 0;    //Snap
//			reference.d4p_y = 0;
//			reference.d4p_z = 0;
			reference.d3p_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[9];     //Jerk
			reference.d3p_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[10];
			reference.d3p_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[11];
			reference.d4p_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[12];    //Snap
			reference.d4p_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[13];
			reference.d4p_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[14];

			reference.psi = TRAJECTORY_GENERATOR_APP_Y.REF_YAW[0];       //Yaw
			reference.d_psi = TRAJECTORY_GENERATOR_APP_Y.REF_YAW[1];
			reference.dd_psi = TRAJECTORY_GENERATOR_APP_Y.REF_YAW[2];

			reference.button = TRAJECTORY_GENERATOR_APP_Y.REF_BUTTONS;   //Button

			reference.timestamp = TRAJECTORY_GENERATOR_APP_Y.REF_TSTAMP; //Tstamp
			/*End Filling Reference        */

			//warnx("Posx: %.2f - Velx: %.2f - Accx: %.2f - Jerk: %.2f - Snap: %.2f", reference.p_x, reference.dp_x, reference.ddp_x, TRAJECTORY_GENERATOR_APP_Y.REF_POS[9],TRAJECTORY_GENERATOR_APP_Y.REF_POS[12]);
			//publishing references
			orb_publish(ORB_ID(unibo_reference), reference_pub_fd, &reference);
			//warnx("Actual yaw: %.3f - YawREF: %.3f - DYawREF: %.3f - D2YawREF: %.3f", TRAJECTORY_GENERATOR_APP_U.PSI, reference.psi, reference.d_psi, reference.dd_psi);

			setpoint.x = reference.p_x;             //publish in vehicle_setpoint
			setpoint.y = reference.p_y;
			setpoint.z = reference.p_z;
			setpoint.yaw = reference.psi;
			orb_publish(ORB_ID(vehicle_local_position_setpoint), setpoint_pub_fd, &setpoint);

			setpoint_triplet.current.type = SETPOINT_TYPE_POSITION;	   //publish in setpoint_triplet
			setpoint_triplet.nav_state = 0; //NAV_STATE_NONE;
			setpoint_triplet.current.lat = reference.p_x;              //lat-->x
			setpoint_triplet.current.lon = reference.p_y;              //lon-->y
			setpoint_triplet.current.alt = reference.p_z;              //alt-->z
			setpoint_triplet.current.yaw = reference.psi;			// IN MAVLINK MESSAGES VENGONO MANIPOLATI. ES: *1e7, lo yaw è trasformato, etc,...TODO check
			setpoint_triplet.previous.valid = true;
			setpoint_triplet.current.valid = true;
			setpoint_triplet.next.valid = true;
			orb_publish(ORB_ID(position_setpoint_triplet), setpoint_triplet_pub_fd, &setpoint_triplet);
		}
		usleep(10000);

	}

	warnx("[unibo_trajectory_ref_daemon] exiting.\n");

	thread_running = false;

	return 0;

}
Exemple #20
0
pthread_t
uorb_receive_start(void)
{
	/* --- SENSORS RAW VALUE --- */
	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
	/* rate limit set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.sensor_sub, 200);	/* 5Hz updates */

	/* --- ATTITUDE VALUE --- */
	mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	/* rate limit set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.att_sub, 200);	/* 5Hz updates */

	/* --- GPS VALUE --- */
	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	orb_set_interval(mavlink_subs.gps_sub, 200);	/* 5Hz updates */

	/* --- HOME POSITION --- */
	mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
	orb_set_interval(mavlink_subs.home_sub, 1000);	/* 1Hz updates */

	/* --- SYSTEM STATE --- */
	status_sub = orb_subscribe(ORB_ID(vehicle_status));
	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */

	/* --- RC CHANNELS VALUE --- */
	rc_sub = orb_subscribe(ORB_ID(rc_channels));
	orb_set_interval(rc_sub, 100);			/* 10Hz updates */

	/* --- RC RAW VALUE --- */
	mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
	orb_set_interval(mavlink_subs.input_rc_sub, 100);

	/* --- GLOBAL POS VALUE --- */
	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */

	/* --- LOCAL POS VALUE --- */
	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */

	/* --- GLOBAL SETPOINT VALUE --- */
	mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
	orb_set_interval(mavlink_subs.spg_sub, 2000);	/* 0.5 Hz updates */

	/* --- LOCAL SETPOINT VALUE --- */
	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */

	/* --- ATTITUDE SETPOINT VALUE --- */
	mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	orb_set_interval(mavlink_subs.spa_sub, 2000);	/* 0.5 Hz updates */

	/* --- RATES SETPOINT VALUE --- */
	mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000);  /* 0.5 Hz updates */

	/* --- ACTUATOR OUTPUTS --- */
	mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
	mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
	mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
	mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
	/* rate limits set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.act_0_sub, 100);	/* 10Hz updates */
	orb_set_interval(mavlink_subs.act_1_sub, 100);	/* 10Hz updates */
	orb_set_interval(mavlink_subs.act_2_sub, 100);	/* 10Hz updates */
	orb_set_interval(mavlink_subs.act_3_sub, 100);	/* 10Hz updates */

	/* --- ACTUATOR ARMED VALUE --- */
	mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	orb_set_interval(mavlink_subs.armed_sub, 100);	/* 10Hz updates */

	/* --- MAPPED MANUAL CONTROL INPUTS --- */
	mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	/* rate limits set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */

	/* --- ACTUATOR CONTROL VALUE --- */
	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */

	/* --- DEBUG VALUE OUTPUT --- */
	mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */

	/* --- FLOW SENSOR --- */
	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */

	/* start the listener loop */
	pthread_attr_t uorb_attr;
	pthread_attr_init(&uorb_attr);

	/* Set stack size, needs less than 2k */
	pthread_attr_setstacksize(&uorb_attr, 2048);

	pthread_t thread;
	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
	return thread;
}
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
{
	calibrate_return result = calibrate_return_ok;

	mag_worker_data_t worker_data;
	
	worker_data.mavlink_fd = mavlink_fd;
	worker_data.done_count = 0;
	worker_data.calibration_points_perside = 40;
	worker_data.calibration_interval_perside_seconds = 20;
	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;

	// Collect: Right-side up, Left Side, Nose down
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
	
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		// Initialize to no subscription
		worker_data.sub_mag[cur_mag] = -1;
		
		// Initialize to no memory allocated
		worker_data.x[cur_mag] = NULL;
		worker_data.y[cur_mag] = NULL;
		worker_data.z[cur_mag] = NULL;
		worker_data.calibration_counter_total[cur_mag] = 0;
	}

	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
	
	char str[30];
	
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
			result = calibrate_return_error;
		}
	}

	
	// Setup subscriptions to mag sensors
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available
				worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
				if (worker_data.sub_mag[cur_mag] < 0) {
					mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
					result = calibrate_return_error;
					break;
				}
			}
		}
	}
	
	// Limit update rate to get equally spaced measurements over time (in ms)
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available
				unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
				
				//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
				orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
			}
		}
		
	}
    
	if (result == calibrate_return_ok) {
		int cancel_sub  = calibrate_cancel_subscribe();

		result = calibrate_from_orientation(mavlink_fd,                         // Mavlink fd to write output
						    cancel_sub,                         // Subscription to vehicle_command for cancel support
						    worker_data.side_data_collected,    // Sides to calibrate
						    mag_calibration_worker,             // Calibration worker
						    &worker_data,			// Opaque data for calibration worked
						    true);				// true: lenient still detection
		calibrate_cancel_unsubscribe(cancel_sub);
	}
	
	// Close subscriptions
	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
		if (worker_data.sub_mag[cur_mag] >= 0) {
			px4_close(worker_data.sub_mag[cur_mag]);
		}
	}
	
	// Calculate calibration values for each mag
	
	
	float sphere_x[max_mags];
	float sphere_y[max_mags];
	float sphere_z[max_mags];
	float sphere_radius[max_mags];
	
	// Sphere fit the data to get calibration values
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available and we should have values for it to calibrate
				
				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
							 worker_data.calibration_counter_total[cur_mag],
							 100, 0.0f,
							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
							 &sphere_radius[cur_mag]);
				
				if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
					result = calibrate_return_error;
				}
			}
		}
	}

	// Print uncalibrated data points
	if (result == calibrate_return_ok) {

		printf("RAW DATA:\n--------------------\n");
		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

			printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
				float x = worker_data.x[cur_mag][i];
				float y = worker_data.y[cur_mag][i];
				float z = worker_data.z[cur_mag][i];
				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
			}

			printf(">>>>>>>\n");
		}

		printf("CALIBRATED DATA:\n--------------------\n");
		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

			printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
				float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
				float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
				float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
			}

			printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
			printf(">>>>>>>\n");
		}
	}
	
	// Data points are no longer needed
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		free(worker_data.x[cur_mag]);
		free(worker_data.y[cur_mag]);
		free(worker_data.z[cur_mag]);
	}
	
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				int fd_mag = -1;
				struct mag_scale mscale;
				
				// Set new scale
				
				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
				fd_mag = px4_open(str, 0);
				if (fd_mag < 0) {
					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
					result = calibrate_return_error;
				}
				
				if (result == calibrate_return_ok) {
					if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
						mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
						result = calibrate_return_error;
					}
				}

				if (result == calibrate_return_ok) {
					mscale.x_offset = sphere_x[cur_mag];
					mscale.y_offset = sphere_y[cur_mag];
					mscale.z_offset = sphere_z[cur_mag];

					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
						result = calibrate_return_error;
					}
				}
				
				// Mag device no longer needed
				if (fd_mag >= 0) {
					px4_close(fd_mag);
				}

				if (result == calibrate_return_ok) {
					bool failed = false;
					
					/* set parameters */
					(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));

					if (failed) {
						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
						result = calibrate_return_error;
					} else {
						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
									     cur_mag,
									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
									     cur_mag,
									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
					}
				}
			}
		}
	}

	return result;
}
Exemple #22
0
int sdlog_thread_main(int argc, char *argv[]) {

	warnx("starting\n");

	if (file_exist(mountpoint) != OK) {
		errx(1, "logging mount point %s not present, exiting.", mountpoint);
	}

	char folder_path[64];
	if (create_logfolder(folder_path))
		errx(1, "unable to create logging folder, exiting.");

	/* create sensorfile */
	int sensorfile = -1;
	int actuator_outputs_file = -1;
	int actuator_controls_file = -1;
	int sysvector_file = -1;
	FILE *gpsfile;
	FILE *blackbox_file;
	// FILE *vehiclefile;

	char path_buf[64] = ""; // string to hold the path to the sensorfile

	warnx("logging to directory %s\n", folder_path);

	/* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */
	sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined");
	if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
		errx(1, "opening %s failed.\n", path_buf);
	}

	// /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */
	// sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0");
	// if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
	// 	errx(1, "opening %s failed.\n", path_buf);
	// }

	/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
	sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
	if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
		errx(1, "opening %s failed.\n", path_buf);
	}

	/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
	sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0");
	if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
		errx(1, "opening %s failed.\n", path_buf);
	}

	/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
	sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
	if (NULL == (gpsfile = fopen(path_buf, "w"))) {
		errx(1, "opening %s failed.\n", path_buf);
	}
	int gpsfile_no = fileno(gpsfile);

	/* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
	sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
	if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
		errx(1, "opening %s failed.\n", path_buf);
	}
	int blackbox_file_no = fileno(blackbox_file);

	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
	/* number of messages */
	const ssize_t fdsc = 25;
	/* Sanity check variable and index */
	ssize_t fdsc_count = 0;
	/* file descriptors to wait for */
	struct pollfd fds[fdsc];


	struct {
		struct sensor_combined_s raw;
		struct vehicle_attitude_s att;
		struct vehicle_attitude_setpoint_s att_sp;
		struct actuator_outputs_s act_outputs;
		struct actuator_controls_s act_controls;
		struct vehicle_command_s cmd;
		struct vehicle_local_position_s local_pos;
		struct vehicle_global_position_s global_pos;
		struct vehicle_gps_position_s gps_pos;
	} buf;
	memset(&buf, 0, sizeof(buf));

	struct {
		int cmd_sub;
		int sensor_sub;
		int att_sub;
		int spa_sub;
		int act_0_sub;
		int controls0_sub;
		int local_pos_sub;
		int global_pos_sub;
		int gps_pos_sub;
	} subs;

	/* --- MANAGEMENT - LOGGING COMMAND --- */
	/* subscribe to ORB for sensors raw */
	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
	fds[fdsc_count].fd = subs.cmd_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- SENSORS RAW VALUE --- */
	/* subscribe to ORB for sensors raw */
	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
	fds[fdsc_count].fd = subs.sensor_sub;
	/* rate-limit raw data updates to 200Hz */
	orb_set_interval(subs.sensor_sub, 5);
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ATTITUDE VALUE --- */
	/* subscribe to ORB for attitude */
	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	fds[fdsc_count].fd = subs.att_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ATTITUDE SETPOINT VALUE --- */
	/* subscribe to ORB for attitude setpoint */
	/* struct already allocated */
	subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	fds[fdsc_count].fd = subs.spa_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/** --- ACTUATOR OUTPUTS --- */
	subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
	fds[fdsc_count].fd = subs.act_0_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ACTUATOR CONTROL VALUE --- */
	/* subscribe to ORB for actuator control */
	subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	fds[fdsc_count].fd = subs.controls0_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- LOCAL POSITION --- */
	/* subscribe to ORB for local position */
	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	fds[fdsc_count].fd = subs.local_pos_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- GLOBAL POSITION --- */
	/* subscribe to ORB for global position */
	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	fds[fdsc_count].fd = subs.global_pos_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- GPS POSITION --- */
	/* subscribe to ORB for global position */
	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	fds[fdsc_count].fd = subs.gps_pos_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* WARNING: If you get the error message below,
	 * then the number of registered messages (fdsc)
	 * differs from the number of messages in the above list.
	 */
	if (fdsc_count > fdsc) {
		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__);
		fdsc_count = fdsc;
	}

	/*
	 * set up poll to block for new data,
	 * wait for a maximum of 1000 ms (1 second)
	 */
	// const int timeout = 1000;

	thread_running = true;

	int poll_count = 0;

	starttime = hrt_absolute_time();

	while (!thread_should_exit) {

		// int poll_ret = poll(fds, fdsc_count, timeout);

		// /* handle the poll result */
		// if (poll_ret == 0) {
		// 	/* XXX this means none of our providers is giving us data - might be an error? */
		// } else if (poll_ret < 0) {
		// 	/* XXX this is seriously bad - should be an emergency */
		// } else {

		// 	int ifds = 0;

		// 	if (poll_count % 5000 == 0) {
		// 		fsync(sensorfile);
		// 		fsync(actuator_outputs_file);
		// 		fsync(actuator_controls_file);
		// 		fsync(blackbox_file_no);
		// 	}

			

		// 	/* --- VEHICLE COMMAND VALUE --- */
		// 	if (fds[ifds++].revents & POLLIN) {
		// 		/* copy command into local buffer */
		// 		orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
		// 		blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
		// 			buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
		// 			(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
		// 	}

		// 	/* --- SENSORS RAW VALUE --- */
		// 	if (fds[ifds++].revents & POLLIN) {

		// 		/* copy sensors raw data into local buffer */
		// 		orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
		// 		/* write out */
		// 		sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
		// 	}

		// 	/* --- ATTITUDE VALUE --- */
		// 	if (fds[ifds++].revents & POLLIN) {

		// 		/* copy attitude data into local buffer */
		// 		orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);

				
		// 	}

		// 	/* --- VEHICLE ATTITUDE SETPOINT --- */
		// 	if (fds[ifds++].revents & POLLIN) {
		// 		/* copy local position data into local buffer */
		// 		orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
				
		// 	}

		// 	/* --- ACTUATOR OUTPUTS 0 --- */
		// 	if (fds[ifds++].revents & POLLIN) {
		// 		/* copy actuator data into local buffer */
		// 		orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
		// 		/* write out */
		// 		// actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.act_outputs, sizeof(buf.act_outputs));
		// 	}

		// 	/* --- ACTUATOR CONTROL --- */
		// 	if (fds[ifds++].revents & POLLIN) {
		// 		orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls);
		// 		/* write out */
		// 		actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls));
		// 	}
		// }

		if (poll_count % 100 == 0) {
			fsync(sysvector_file);
		}

		poll_count++;


		/* copy sensors raw data into local buffer */
		orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
		orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls);
		/* copy actuator data into local buffer */
		orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
		orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
		orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
		orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
		orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
		orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);

		#pragma pack(push, 1)
		struct {
			uint64_t timestamp; //[us]
			float gyro[3]; //[rad/s]
			float accel[3]; //[m/s^2]
			float mag[3]; //[gauss]
			float baro; //pressure [millibar]
			float baro_alt; //altitude above MSL [meter]
			float baro_temp; //[degree celcius]
			float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
			float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
			float vbat; //battery voltage in [volt]
			float adc[3]; //remaining auxiliary ADC ports [volt]
			float local_position[3]; //tangent plane mapping into x,y,z [m]
			int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
			float attitude[3]; //pitch, roll, yaw [rad]
			float rotMatrix[9]; //unitvectors
		} sysvector = {
			.timestamp = buf.raw.timestamp,
			.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
			.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
			.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
			.baro = buf.raw.baro_pres_mbar,
			.baro_alt = buf.raw.baro_alt_meter,
			.baro_temp = buf.raw.baro_temp_celcius,
			.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
			.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
					buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
			.vbat = buf.raw.battery_voltage_v,
			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
			.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
			.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
			.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
			.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}
		};
		#pragma pack(pop)

		sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));

		usleep(10000);   //10000 corresponds in reality to ca. 76 Hz
	}

	fsync(sysvector_file);

	print_sdlog_status();

	warnx("exiting.\n");

	close(sensorfile);
	close(actuator_outputs_file);
	close(actuator_controls_file);
	fclose(gpsfile);
	fclose(blackbox_file);

	thread_running = false;

	return 0;
}

void print_sdlog_status()
{
	unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
	float mebibytes = bytes / 1024.0f / 1024.0f;
	float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;

	warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
}
/*
 * Nonliner Exciplicit complementary filter (ECF), attitude estimator main function.
 *
 * Estimates the attitude once started.
 *
 * @param argc number of commandline arguments (plus command name)
 * @param argv strings containing the arguments
 */
int unibo_att_esti_ECF_s_thread_main(int argc, char *argv[])
{
	const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds

	//! Time constant
	float dt = 0.005f;
	
	/* output euler angles */
	float euler[3] = {0.0f, 0.0f, 0.0f};
	
	/* Initialization */
	float Rot_matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };		/**< init: identity matrix */
	float acc[3] = {0.0f, 0.0f, 0.0f};
	float gyro[3] = {0.0f, 0.0f, 0.0f};
	float mag[3] = {0.0f, 0.0f, 0.0f};

	warnx("main thread started");

	struct sensor_combined_s raw;
	memset(&raw, 0, sizeof(raw));

	//! Initialize attitude vehicle uORB message.
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));

	struct vehicle_control_mode_s control_mode;
	memset(&control_mode, 0, sizeof(control_mode));

	uint64_t last_data = 0;
	uint64_t last_measurement = 0;

	/* subscribe to raw data */
	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
	orb_set_interval(sub_raw, 3);

	/* subscribe to param changes */
	int sub_params = orb_subscribe(ORB_ID(parameter_update));

	/* subscribe to control mode */
	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));

	/* advertise attitude */
	//orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
	//orb_advert_t att_pub = -1;
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

	int loopcounter = 0;
	int printcounter = 0;

	thread_running = true;

	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
	// XXX write this out to perf regs

	/* keep track of sensor updates */
	uint32_t sensor_last_count[3] = {0, 0, 0};
	uint64_t sensor_last_timestamp[3] = {0, 0, 0};

	struct unibo_att_esti_ECF_s_params so3_comp_params;
	struct unibo_att_esti_ECF_s_param_handles so3_comp_param_handles;

	/* initialize parameter handles */
	parameters_init(&so3_comp_param_handles);
	parameters_update(&so3_comp_param_handles, &so3_comp_params);

	uint64_t start_time = hrt_absolute_time();
	bool initialized = false;
	bool state_initialized = false;

	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
	unsigned offset_count = 0;

	/* register the perf counter */
	perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "unibo_att_esti_ECF_s");

	/*-----------------------------------------------
	 *                     Main loop
	 * ----------------------------------------------*/
	while (!thread_should_exit) {

		struct pollfd fds[2];
		fds[0].fd = sub_raw;
		fds[0].events = POLLIN;
		fds[1].fd = sub_params;
		fds[1].events = POLLIN;
		int ret = poll(fds, 2, 1000);

		if (ret < 0) {
			/* XXX this is seriously bad - should be an emergency */
		} else if (ret == 0) {
			/* check if we're in HIL - not getting sensor data is fine then */
			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);

			if (!control_mode.flag_system_hil_enabled) {
				warnx("WARNING: Not getting sensors - sensor app running?");
			}
		} else {
			/* only update parameters if they changed */
			if (fds[1].revents & POLLIN) {
				/* read from param to clear updated flag */
				struct parameter_update_s update;
				orb_copy(ORB_ID(parameter_update), sub_params, &update);

				/* update parameters */
				parameters_update(&so3_comp_param_handles, &so3_comp_params);
			}

			/* only run filter if sensor values changed */
			if (fds[0].revents & POLLIN) {

				/* get latest measurements */
				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);

				if (!initialized) {

					gyro_offsets[0] += raw.gyro_rad_s[0];
					gyro_offsets[1] += raw.gyro_rad_s[1];
					gyro_offsets[2] += raw.gyro_rad_s[2];
					offset_count++;

					if (hrt_absolute_time() > start_time + 3000000l) {
						initialized = true;
						gyro_offsets[0] /= offset_count;
						gyro_offsets[1] /= offset_count;
						gyro_offsets[2] /= offset_count;
						warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]);
					}

				} else {

					perf_begin(so3_comp_loop_perf);

					/* Calculate data time difference in seconds */
					dt = (raw.timestamp - last_measurement) / 1000000.0f;
					last_measurement = raw.timestamp;
					uint8_t update_vect[3] = {0, 0, 0};

					/* Fill in gyro measurements */
					if (sensor_last_timestamp[0] != raw.timestamp) {
						update_vect[0] = 1;
						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
						sensor_last_timestamp[0] = raw.timestamp;
					}

					gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
					gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
					gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];

					/* update accelerometer measurements */
					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
						update_vect[1] = 1;
						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
					}

					acc[0] = raw.accelerometer_m_s2[0];
					acc[1] = raw.accelerometer_m_s2[1];
					acc[2] = raw.accelerometer_m_s2[2];

					/* update magnetometer measurements */
					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
						update_vect[2] = 1;
						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
					}

					mag[0] = raw.magnetometer_ga[0];
					mag[1] = raw.magnetometer_ga[1];
					mag[2] = raw.magnetometer_ga[2];

					/* initialize with good values once we have a reasonable dt estimate */
					if (!state_initialized && dt < 0.05f && dt > 0.001f) {
						state_initialized = true;
						warnx("state initialized");
					}

					/* do not execute the filter if not initialized */
					if (!state_initialized) {
						continue;
					}

					uint64_t timing_start = hrt_absolute_time();

					// NOTE : Accelerometer is reversed.
					// Because proper mount of PX4 will give you a reversed accelerometer readings.
					standardECFnew(gyro[0], gyro[1], gyro[2],
										-acc[0], -acc[1], -acc[2],
										mag[0], mag[1], mag[2],
										so3_comp_params.Ka,
										so3_comp_params.Km,
										so3_comp_params.Kp,
										so3_comp_params.Ki,
										dt);

					// direction cosine matrix of 1-2-3 sequence
					// Resulted Rotation matrix convert from Inertial frame to body frame
					Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;	// R11
					Rot_matrix[1] = 2.f * (q1*q2 + q0*q3);		// R12
					Rot_matrix[2] = 2.f * (q1*q3 - q0*q2);		// R13
					Rot_matrix[3] = 2.f * (q1*q2 - q0*q3);		// R21
					Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;	// R22
					Rot_matrix[5] = 2.f * (q2*q3 + q0*q1);		// R23
					Rot_matrix[6] = 2.f * (q1*q3 + q0*q2);		// R31
					Rot_matrix[7] = 2.f * (q2*q3 - q0*q1);		// R32
					Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;	// R33

					// Euler Angles using Unit Quaternions;

					euler[0]= atan2f((2.0f*(q2q3+q0q1)),1.0f-2.0f*(q1q1+q2q2));		// Roll
					euler[1]= asinf(-2.0f*(q1q3-q0q2));								// Pitch
					euler[2]= atan2f((2.0f*(q1q2+q0q3)),1.0f-2.0f*(q2q2+q3q3));		// Yaw
					
					/* swap values for next iteration, check for fatal inputs */
					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
						// Publish only finite euler angles
						att.roll = euler[0] - so3_comp_params.roll_off;
						att.pitch = euler[1] - so3_comp_params.pitch_off;
						att.yaw = euler[2] - so3_comp_params.yaw_off;
					} else {
						/* due to inputs or numerical failure the output is invalid, skip it */
						// Due to inputs or numerical failure the output is invalid
						warnx("infinite euler angles, rotation matrix:");
						warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]);
						warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]);
						warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]);
						// Don't publish anything
						continue;
					}

					if (last_data > 0 && raw.timestamp > last_data + 12000) {
						warnx("sensor data missed");
					}

					last_data = raw.timestamp;

					/* send out */
					att.timestamp = raw.timestamp;
					
					// Quaternion
					att.q[0] = q0;
					att.q[1] = q1;
					att.q[2] = q2;
					att.q[3] = q3;
					att.q_valid = true;

					// Euler angle rate. But it needs to be investigated again.
					/*
					att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
					att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
					att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3);
					*/
					att.rollspeed = gyro[0];
					att.pitchspeed = gyro[1];
					att.yawspeed = gyro[2];

					att.rollacc = 0;
					att.pitchacc = 0;
					att.yawacc = 0;

					/* TODO: Bias estimation required */
					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));

					/* copy rotation matrix */
					memcpy(&att.R, Rot_matrix, sizeof(float)*9);
					att.R_valid = true;
					
					// Publish
					if (att_pub > 0) {
						orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
					} else {
						warnx("NaN in roll/pitch/yaw estimate!");
						 orb_advertise(ORB_ID(vehicle_attitude), &att);
					}

					perf_end(so3_comp_loop_perf);
				}
			}
		}

		loopcounter++;
	}
// here the main loop stop

	thread_running = false;

	return 0;
}
void
Navigator::task_main()
{
	/* inform about start */
	warnx("Initializing..");

	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
	_geofence.setMavlinkFd(_mavlink_fd);

	/* Try to load the geofence:
	 * if /fs/microsd/etc/geofence.txt load from this file
	 * else clear geofence data in datamanager */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		warnx("Try to load geofence.txt");
		_geofence.loadFromFile(GEOFENCE_FILENAME);

	} else {
		mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
		if (_geofence.clearDm() > 0)
			warnx("Geofence cleared");
		else
			warnx("Could not clear geofence");
	}

	/* do subscriptions */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));

	/* copy all topics first time */
	vehicle_status_update();
	vehicle_control_mode_update();
	global_position_update();
	gps_position_update();
	sensor_combined_update();
	home_position_update();
	navigation_capabilities_update();
	params_update();

	/* rate limit position and sensor updates to 50 Hz */
	orb_set_interval(_global_pos_sub, 20);
	orb_set_interval(_sensor_combined_sub, 20);

	hrt_abstime mavlink_open_time = 0;
	const hrt_abstime mavlink_open_interval = 500000;

	/* wakeup source(s) */
	struct pollfd fds[8];

	/* Setup of loop */
	fds[0].fd = _global_pos_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _home_pos_sub;
	fds[1].events = POLLIN;
	fds[2].fd = _capabilities_sub;
	fds[2].events = POLLIN;
	fds[3].fd = _vstatus_sub;
	fds[3].events = POLLIN;
	fds[4].fd = _control_mode_sub;
	fds[4].events = POLLIN;
	fds[5].fd = _param_update_sub;
	fds[5].events = POLLIN;
	fds[6].fd = _sensor_combined_sub;
	fds[6].events = POLLIN;
	fds[7].fd = _gps_pos_sub;
	fds[7].events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		if (pret == 0) {
			/* timed out - periodic check for _task_should_exit, etc. */
			continue;

		} else if (pret < 0) {
			/* this is undesirable but not much we can do - might want to flag unhappy status */
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
			/* try to reopen the mavlink log device with specified interval */
			mavlink_open_time = hrt_abstime() + mavlink_open_interval;
			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
		}

		static bool have_geofence_position_data = false;

		/* gps updated */
		if (fds[7].revents & POLLIN) {
			gps_position_update();
			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
				have_geofence_position_data = true;
			}
		}

		/* sensors combined updated */
		if (fds[6].revents & POLLIN) {
			sensor_combined_update();
		}

		/* parameters updated */
		if (fds[5].revents & POLLIN) {
			params_update();
			updateParams();
		}

		/* vehicle control mode updated */
		if (fds[4].revents & POLLIN) {
			vehicle_control_mode_update();
		}

		/* vehicle status updated */
		if (fds[3].revents & POLLIN) {
			vehicle_status_update();
		}

		/* navigation capabilities updated */
		if (fds[2].revents & POLLIN) {
			navigation_capabilities_update();
		}

		/* home position updated */
		if (fds[1].revents & POLLIN) {
			home_position_update();
		}

		/* global position updated */
		if (fds[0].revents & POLLIN) {
			global_position_update();
			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
				have_geofence_position_data = true;
			}
		}

		/* Check geofence violation */
		static hrt_abstime last_geofence_check = 0;
		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
			last_geofence_check = hrt_absolute_time();
			have_geofence_position_data = false;
			if (!inside) {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = true;
				publish_geofence_result();

				/* Issue a warning about the geofence violation once */
				if (!_geofence_violation_warning_sent) {
					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
					_geofence_violation_warning_sent = true;
				}
			} else {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = false;
				publish_geofence_result();
				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}
		}

		/* Do stuff according to navigation state set by commander */
		switch (_vstatus.nav_state) {
			case NAVIGATION_STATE_MANUAL:
			case NAVIGATION_STATE_ACRO:
			case NAVIGATION_STATE_ALTCTL:
			case NAVIGATION_STATE_POSCTL:
			case NAVIGATION_STATE_LAND:
			case NAVIGATION_STATE_TERMINATION:
			case NAVIGATION_STATE_OFFBOARD:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
			case NAVIGATION_STATE_AUTO_MISSION:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_mission;
				break;
			case NAVIGATION_STATE_AUTO_LOITER:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_loiter;
				break;
			case NAVIGATION_STATE_AUTO_RCRECOVER:
				_pos_sp_triplet_published_invalid_once = false;
				if (_param_rcloss_obc.get() != 0) {
					_navigation_mode = &_rcLoss;
				} else {
					_navigation_mode = &_rtl;
				}
				break;
			case NAVIGATION_STATE_AUTO_RTL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_rtl;
				break;
			case NAVIGATION_STATE_AUTO_RTGS:
				/* Use complex data link loss mode only when enabled via param
				* otherwise use rtl */
				_pos_sp_triplet_published_invalid_once = false;
				if (_param_datalinkloss_obc.get() != 0) {
					_navigation_mode = &_dataLinkLoss;
				} else {
					_navigation_mode = &_rtl;
				}
				break;
			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_engineFailure;
				break;
			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_gpsFailure;
				break;
			default:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
		}

		/* iterate through navigation modes and set active/inactive for each */
		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
		}

		/* if nothing is running, set position setpoint triplet invalid once */
		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
			_pos_sp_triplet_published_invalid_once = true;
			_pos_sp_triplet.previous.valid = false;
			_pos_sp_triplet.current.valid = false;
			_pos_sp_triplet.next.valid = false;
			_pos_sp_triplet_updated = true;
		}

		if (_pos_sp_triplet_updated) {
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

		if (_mission_result_updated) {
			publish_mission_result();
			_mission_result_updated = false;
		}

		perf_end(_loop_perf);
	}
	warnx("exiting.");

	_navigator_task = -1;
	_exit(0);
}
Exemple #25
0
void
BlinkM::led()
{

	static int vehicle_status_sub_fd;
	static int vehicle_gps_position_sub_fd;

	static int num_of_cells = 0;
	static int detected_cells_runcount = 0;

	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
	static int t_led_blink = 0;
	static int led_thread_runcount=0;
	static int led_interval = 1000;

	static int no_data_vehicle_status = 0;
	static int no_data_vehicle_gps_position = 0;

	static bool topic_initialized = false;
	static bool detected_cells_blinked = false;
	static bool led_thread_ready = true;

	int num_of_used_sats = 0;

	if(!topic_initialized) {
		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
		orb_set_interval(vehicle_status_sub_fd, 1000);

		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
		orb_set_interval(vehicle_gps_position_sub_fd, 1000);

		topic_initialized = true;
	}

	if(led_thread_ready == true) {
		if(!detected_cells_blinked) {
			if(num_of_cells > 0) {
				t_led_color[0] = LED_PURPLE;
			}
			if(num_of_cells > 1) {
				t_led_color[1] = LED_PURPLE;
			}
			if(num_of_cells > 2) {
				t_led_color[2] = LED_PURPLE;
			}
			if(num_of_cells > 3) {
				t_led_color[3] = LED_PURPLE;
			}
			if(num_of_cells > 4) {
				t_led_color[4] = LED_PURPLE;
			}
			t_led_color[5] = LED_OFF;
			t_led_color[6] = LED_OFF;
			t_led_color[7] = LED_OFF;
			t_led_blink = LED_BLINK;
		} else {
			t_led_color[0] = led_color_1;
			t_led_color[1] = led_color_2;
			t_led_color[2] = led_color_3;
			t_led_color[3] = led_color_4;
			t_led_color[4] = led_color_5;
			t_led_color[5] = led_color_6;
			t_led_color[6] = led_color_7;
			t_led_color[7] = led_color_8;
			t_led_blink = led_blink;
		}
		led_thread_ready = false;
	}

	if (led_thread_runcount & 1) {
		if (t_led_blink)
			setLEDColor(LED_OFF);
		led_interval = LED_OFFTIME;
	} else {
		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
		led_interval = LED_ONTIME;
	}

	if (led_thread_runcount == 15) {
		/* obtained data for the first file descriptor */
		struct vehicle_status_s vehicle_status_raw;
		struct vehicle_gps_position_s vehicle_gps_position_raw;

		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));

		bool new_data_vehicle_status;
		bool new_data_vehicle_gps_position;

		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);

		if (new_data_vehicle_status) {
			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
			no_data_vehicle_status = 0;
		} else {
			no_data_vehicle_status++;
			if(no_data_vehicle_status >= 3)
				no_data_vehicle_status = 3;
		}

		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);

		if (new_data_vehicle_gps_position) {
			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
			no_data_vehicle_gps_position = 0;
		} else {
			no_data_vehicle_gps_position++;
			if(no_data_vehicle_gps_position >= 3)
				no_data_vehicle_gps_position = 3;
		}



		/* get number of used satellites in navigation */
		num_of_used_sats = 0;
		//for(int satloop=0; satloop<20; satloop++) {
		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
				num_of_used_sats++;
			}
		}

		if(new_data_vehicle_status || no_data_vehicle_status < 3){
			if(num_of_cells == 0) {
				/* looking for lipo cells that are connected */
				printf("<blinkm> checking cells\n");
				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
				}
				printf("<blinkm> cells found:%u\n", num_of_cells);

			} else {
				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
					/* LED Pattern for battery low warning */
					led_color_1 = LED_YELLOW;
					led_color_2 = LED_YELLOW;
					led_color_3 = LED_YELLOW;
					led_color_4 = LED_YELLOW;
					led_color_5 = LED_YELLOW;
					led_color_6 = LED_YELLOW;
					led_color_7 = LED_YELLOW;
					led_color_8 = LED_YELLOW;
					led_blink = LED_BLINK;

				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
					/* LED Pattern for battery critical alerting */
					led_color_1 = LED_RED;
					led_color_2 = LED_RED;
					led_color_3 = LED_RED;
					led_color_4 = LED_RED;
					led_color_5 = LED_RED;
					led_color_6 = LED_RED;
					led_color_7 = LED_RED;
					led_color_8 = LED_RED;
					led_blink = LED_BLINK;

				} else {
					/* no battery warnings here */

					if(vehicle_status_raw.flag_system_armed == false) {
						/* system not armed */
						led_color_1 = LED_RED;
						led_color_2 = LED_RED;
						led_color_3 = LED_RED;
						led_color_4 = LED_RED;
						led_color_5 = LED_RED;
						led_color_6 = LED_RED;
						led_color_7 = LED_RED;
						led_color_8 = LED_RED;
						led_blink = LED_NOBLINK;

					} else {
						/* armed system - initial led pattern */
						led_color_1 = LED_RED;
						led_color_2 = LED_RED;
						led_color_3 = LED_RED;
						led_color_4 = LED_OFF;
						led_color_5 = LED_OFF;
						led_color_6 = LED_OFF;
						led_color_7 = LED_OFF;
						led_color_8 = LED_OFF;
						led_blink = LED_BLINK;

						/* handle 4th led - flightmode indicator */
						switch((int)vehicle_status_raw.flight_mode) {
							case VEHICLE_FLIGHT_MODE_MANUAL:
								led_color_4 = LED_AMBER;
								break;

							case VEHICLE_FLIGHT_MODE_STAB:
								led_color_4 = LED_YELLOW;
								break;

							case VEHICLE_FLIGHT_MODE_HOLD:
								led_color_4 = LED_BLUE;
								break;

							case VEHICLE_FLIGHT_MODE_AUTO:
								led_color_4 = LED_GREEN;
								break;
						}

						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
							/* handling used sat´s */
							if(num_of_used_sats >= 7) {
								led_color_1 = LED_OFF;
								led_color_2 = LED_OFF;
								led_color_3 = LED_OFF;
							} else if(num_of_used_sats == 6) {
								led_color_2 = LED_OFF;
								led_color_3 = LED_OFF;
							} else if(num_of_used_sats == 5) {
								led_color_3 = LED_OFF;
							}

						} else {
							/* no vehicle_gps_position data */
							led_color_1 = LED_WHITE;
							led_color_2 = LED_WHITE;
							led_color_3 = LED_WHITE;

						}

					}
				}
			}
		} else {
			/* LED Pattern for general Error - no vehicle_status can retrieved */
			led_color_1 = LED_WHITE;
			led_color_2 = LED_WHITE;
			led_color_3 = LED_WHITE;
			led_color_4 = LED_WHITE;
			led_color_5 = LED_WHITE;
			led_color_6 = LED_WHITE;
			led_color_7 = LED_WHITE;
			led_color_8 = LED_WHITE;
			led_blink = LED_BLINK;

		}

		/*
		printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
		vehicle_status_raw.voltage_battery,
		vehicle_status_raw.flag_system_armed,
		vehicle_status_raw.flight_mode,
		num_of_cells,
		no_data_vehicle_status,
		no_data_vehicle_gps_position,
		num_of_used_sats,
		vehicle_gps_position_raw.fix_type,
		vehicle_gps_position_raw.satellites_visible);
		*/

		led_thread_runcount=0;
		led_thread_ready = true;
		led_interval = LED_OFFTIME;

		if(detected_cells_runcount < 4){
			detected_cells_runcount++;
		} else {
			detected_cells_blinked = true;
		}

	} else {
		led_thread_runcount++;
	}

	if(systemstate_run == true) {
		/* re-queue ourselves to run again later */
		work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
	} else {
		stop_script();
		set_rgb(0,0,0);
	}
}
/*
 * EKF Attitude Estimator main function.
 *
 * Estimates the attitude recursively once started.
 *
 * @param argc number of commandline arguments (plus command name)
 * @param argv strings containing the arguments
 */
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{

    const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds

    float dt = 0.005f;
    /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
    float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
    float x_aposteriori_k[12];		/**< states */
    float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
                                  0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
                                  0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
                                  0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
                                  0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
                                  0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
                                  0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
                                  0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
                                  0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
                                  0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
                                  0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
                                  0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
                                 }; /**< init: diagonal matrix with big values */

    float x_aposteriori[12];
    float P_aposteriori[144];

    /* output euler angles */
    float euler[3] = {0.0f, 0.0f, 0.0f};

    float Rot_matrix[9] = {1.f,  0,  0,
                           0,  1.f,  0,
                           0,  0,  1.f
                          };		/**< init: identity matrix */

    // print text
    printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
    fflush(stdout);

    int overloadcounter = 19;

    /* Initialize filter */
    attitudeKalmanfilter_initialize();

    /* store start time to guard against too slow update rates */
    uint64_t last_run = hrt_absolute_time();

    struct sensor_combined_s raw;
    memset(&raw, 0, sizeof(raw));
    struct vehicle_gps_position_s gps;
    memset(&gps, 0, sizeof(gps));
    struct vehicle_global_position_s global_pos;
    memset(&global_pos, 0, sizeof(global_pos));
    struct vehicle_attitude_s att;
    memset(&att, 0, sizeof(att));
    struct vehicle_control_mode_s control_mode;
    memset(&control_mode, 0, sizeof(control_mode));
    struct vehicle_vicon_position_s vicon_pos;  // Added by Ross Allen
    memset(&vicon_pos, 0, sizeof(vicon_pos));

    uint64_t last_data = 0;
    uint64_t last_measurement = 0;
    uint64_t last_vel_t = 0;

    /* Vicon parameters - Added by Ross Allen */
    bool vicon_valid = false;
    //~ static const hrt_abstime vicon_topic_timeout = 500000;		// vicon topic timeout = 0.25s

    /* current velocity */
    math::Vector<3> vel;
    vel.zero();
    /* previous velocity */
    math::Vector<3> vel_prev;
    vel_prev.zero();
    /* actual acceleration (by GPS velocity) in body frame */
    math::Vector<3> acc;
    acc.zero();
    /* rotation matrix */
    math::Matrix<3, 3> R;
    R.identity();

    /* subscribe to raw data */
    int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
    /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
    orb_set_interval(sub_raw, 3);

    /* subscribe to GPS */
    int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));

    /* subscribe to GPS */
    int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));

    /* subscribe to param changes */
    int sub_params = orb_subscribe(ORB_ID(parameter_update));

    /* subscribe to control mode*/
    int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));

    /* subscribe to vicon position */
    int vehicle_vicon_position_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); // Added by Ross Allen

    /* advertise attitude */
    orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);

    int loopcounter = 0;

    thread_running = true;

    /* advertise debug value */
    // struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
    // orb_advert_t pub_dbg = -1;

    /* keep track of sensor updates */
    uint64_t sensor_last_timestamp[3] = {0, 0, 0};

    struct attitude_estimator_ekf_params ekf_params;

    struct attitude_estimator_ekf_param_handles ekf_param_handles;

    /* initialize parameter handles */
    parameters_init(&ekf_param_handles);

    bool initialized = false;

    float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };

    /* magnetic declination, in radians */
    float mag_decl = 0.0f;

    /* rotation matrix for magnetic declination */
    math::Matrix<3, 3> R_decl;
    R_decl.identity();

    /* register the perf counter */
    perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");

    /* Main loop*/
    while (!thread_should_exit) {

        struct pollfd fds[2];
        fds[0].fd = sub_raw;
        fds[0].events = POLLIN;
        fds[1].fd = sub_params;
        fds[1].events = POLLIN;
        int ret = poll(fds, 2, 1000);

        /* Added by Ross Allen */
        //~ hrt_abstime t = hrt_absolute_time();
        bool updated;

        if (ret < 0) {
            /* XXX this is seriously bad - should be an emergency */
        } else if (ret == 0) {
            /* check if we're in HIL - not getting sensor data is fine then */
            orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);

            if (!control_mode.flag_system_hil_enabled) {
                fprintf(stderr,
                        "[att ekf] WARNING: Not getting sensors - sensor app running?\n");
            }

        } else {

            /* only update parameters if they changed */
            if (fds[1].revents & POLLIN) {
                /* read from param to clear updated flag */
                struct parameter_update_s update;
                orb_copy(ORB_ID(parameter_update), sub_params, &update);

                /* update parameters */
                parameters_update(&ekf_param_handles, &ekf_params);
            }

            /* only run filter if sensor values changed */
            if (fds[0].revents & POLLIN) {

                /* get latest measurements */
                orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);

                bool gps_updated;
                orb_check(sub_gps, &gps_updated);
                if (gps_updated) {
                    orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);

                    if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
                        mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

                        /* update mag declination rotation matrix */
                        R_decl.from_euler(0.0f, 0.0f, mag_decl);
                    }
                }

                bool global_pos_updated;
                orb_check(sub_global_pos, &global_pos_updated);
                if (global_pos_updated) {
                    orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
                }

                if (!initialized) {
                    // XXX disabling init for now
                    initialized = true;

                    // gyro_offsets[0] += raw.gyro_rad_s[0];
                    // gyro_offsets[1] += raw.gyro_rad_s[1];
                    // gyro_offsets[2] += raw.gyro_rad_s[2];
                    // offset_count++;

                    // if (hrt_absolute_time() - start_time > 3000000LL) {
                    // 	initialized = true;
                    // 	gyro_offsets[0] /= offset_count;
                    // 	gyro_offsets[1] /= offset_count;
                    // 	gyro_offsets[2] /= offset_count;
                    // }

                } else {

                    perf_begin(ekf_loop_perf);

                    /* Calculate data time difference in seconds */
                    dt = (raw.timestamp - last_measurement) / 1000000.0f;
                    last_measurement = raw.timestamp;
                    uint8_t update_vect[3] = {0, 0, 0};

                    /* Fill in gyro measurements */
                    if (sensor_last_timestamp[0] != raw.timestamp) {
                        update_vect[0] = 1;
                        // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
                        sensor_last_timestamp[0] = raw.timestamp;
                    }

                    z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
                    z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
                    z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];

                    /* update accelerometer measurements */
                    if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
                        update_vect[1] = 1;
                        // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
                        sensor_last_timestamp[1] = raw.accelerometer_timestamp;
                    }

                    hrt_abstime vel_t = 0;
                    bool vel_valid = false;
                    if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
                        vel_valid = true;
                        if (gps_updated) {
                            vel_t = gps.timestamp_velocity;
                            vel(0) = gps.vel_n_m_s;
                            vel(1) = gps.vel_e_m_s;
                            vel(2) = gps.vel_d_m_s;
                        }

                    } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
                        vel_valid = true;
                        if (global_pos_updated) {
                            vel_t = global_pos.timestamp;
                            vel(0) = global_pos.vel_n;
                            vel(1) = global_pos.vel_e;
                            vel(2) = global_pos.vel_d;
                        }
                    }

                    if (vel_valid) {
                        /* velocity is valid */
                        if (vel_t != 0) {
                            /* velocity updated */
                            if (last_vel_t != 0 && vel_t != last_vel_t) {
                                float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
                                /* calculate acceleration in body frame */
                                acc = R.transposed() * ((vel - vel_prev) / vel_dt);
                            }
                            last_vel_t = vel_t;
                            vel_prev = vel;
                        }

                    } else {
                        /* velocity is valid, reset acceleration */
                        acc.zero();
                        vel_prev.zero();
                        last_vel_t = 0;
                    }

                    z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
                    z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
                    z_k[5] = raw.accelerometer_m_s2[2] - acc(2);

                    /* update magnetometer measurements */
                    if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
                        update_vect[2] = 1;
                        // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
                        sensor_last_timestamp[2] = raw.magnetometer_timestamp;
                    }

                    z_k[6] = raw.magnetometer_ga[0];
                    z_k[7] = raw.magnetometer_ga[1];
                    z_k[8] = raw.magnetometer_ga[2];

                    uint64_t now = hrt_absolute_time();
                    unsigned int time_elapsed = now - last_run;
                    last_run = now;

                    if (time_elapsed > loop_interval_alarm) {
                        //TODO: add warning, cpu overload here
                        // if (overloadcounter == 20) {
                        // 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
                        // 	overloadcounter = 0;
                        // }

                        overloadcounter++;
                    }

                    static bool const_initialized = false;

                    /* initialize with good values once we have a reasonable dt estimate */
                    if (!const_initialized && dt < 0.05f && dt > 0.001f) {
                        dt = 0.005f;
                        parameters_update(&ekf_param_handles, &ekf_params);

                        /* update mag declination rotation matrix */
                        if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
                            mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

                        } else {
                            mag_decl = ekf_params.mag_decl;
                        }

                        /* update mag declination rotation matrix */
                        R_decl.from_euler(0.0f, 0.0f, mag_decl);

                        x_aposteriori_k[0] = z_k[0];
                        x_aposteriori_k[1] = z_k[1];
                        x_aposteriori_k[2] = z_k[2];
                        x_aposteriori_k[3] = 0.0f;
                        x_aposteriori_k[4] = 0.0f;
                        x_aposteriori_k[5] = 0.0f;
                        x_aposteriori_k[6] = z_k[3];
                        x_aposteriori_k[7] = z_k[4];
                        x_aposteriori_k[8] = z_k[5];
                        x_aposteriori_k[9] = z_k[6];
                        x_aposteriori_k[10] = z_k[7];
                        x_aposteriori_k[11] = z_k[8];

                        const_initialized = true;
                    }

                    /* do not execute the filter if not initialized */
                    if (!const_initialized) {
                        continue;
                    }

                    attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
                                         euler, Rot_matrix, x_aposteriori, P_aposteriori);

                    /* swap values for next iteration, check for fatal inputs */
                    if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
                        memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
                        memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));

                    } else {
                        /* due to inputs or numerical failure the output is invalid, skip it */
                        continue;
                    }

                    if (last_data > 0 && raw.timestamp - last_data > 30000)
                        printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);

                    last_data = raw.timestamp;

                    /* Check Vicon for attitude data*/
                    /* Added by Ross Allen */
                    orb_check(vehicle_vicon_position_sub, &updated);

                    if (updated) {
                        orb_copy(ORB_ID(vehicle_vicon_position), vehicle_vicon_position_sub, &vicon_pos);
                        vicon_valid = vicon_pos.valid;
                    }

                    //~ if (vicon_valid && (t > (vicon_pos.timestamp + vicon_topic_timeout))) {
                    //~ vicon_valid = false;
                    //~ warnx("VICON timeout");
                    //~ }

                    /* send out */
                    att.timestamp = raw.timestamp;

                    att.roll = euler[0];
                    att.pitch = euler[1];
                    att.yaw = euler[2] + mag_decl;

                    /* Use vicon yaw if valid and just overwrite*/
                    /* Added by Ross Allen */
                    if(vicon_valid) {
                        att.yaw = vicon_pos.yaw;
                    }

                    att.rollspeed = x_aposteriori[0];
                    att.pitchspeed = x_aposteriori[1];
                    att.yawspeed = x_aposteriori[2];
                    att.rollacc = x_aposteriori[3];
                    att.pitchacc = x_aposteriori[4];
                    att.yawacc = x_aposteriori[5];

                    att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
                    att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
                    att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);

                    /* copy offsets */
                    memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

                    /* magnetic declination */

                    math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
                    R = R_decl * R_body;

                    /* copy rotation matrix */
                    memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
                    att.R_valid = true;

                    if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
                        // Broadcast
                        orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

                    } else {
                        warnx("NaN in roll/pitch/yaw estimate!");
                    }

                    perf_end(ekf_loop_perf);
                }
            }
        }

        loopcounter++;
    }

    thread_running = false;

    return 0;
}
Exemple #27
0
void
PX4FMU::task_main()
{
	/* force a reset of the update rate */
	_current_update_rate = 0;

	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_param_sub = orb_subscribe(ORB_ID(parameter_update));

	/* advertise the mixed control outputs */
	actuator_outputs_s outputs;
	memset(&outputs, 0, sizeof(outputs));

#ifdef HRT_PPM_CHANNEL
	// rc input, published to ORB
	struct rc_input_values rc_in;
	orb_advert_t to_input_rc = 0;

	memset(&rc_in, 0, sizeof(rc_in));
	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
#endif

	/* initialize PWM limit lib */
	pwm_limit_init(&_pwm_limit);

	update_pwm_rev_mask();

	/* loop until killed */
	while (!_task_should_exit) {
		if (_groups_subscribed != _groups_required) {
			subscribe();
			_groups_subscribed = _groups_required;
			/* force setting update rate */
			_current_update_rate = 0;
		}

		/*
		 * Adjust actuator topic update rate to keep up with
		 * the highest servo update rate configured.
		 *
		 * We always mix at max rate; some channels may update slower.
		 */
		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;

		if (_current_update_rate != max_rate) {
			_current_update_rate = max_rate;
			int update_rate_in_ms = int(1000 / _current_update_rate);

			/* reject faster than 500 Hz updates */
			if (update_rate_in_ms < 2) {
				update_rate_in_ms = 2;
			}

			/* reject slower than 10 Hz updates */
			if (update_rate_in_ms > 100) {
				update_rate_in_ms = 100;
			}

			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
				if (_control_subs[i] > 0) {
					orb_set_interval(_control_subs[i], update_rate_in_ms);
				}
			}

			// set to current max rate, even if we are actually checking slower/faster
			_current_update_rate = max_rate;
		}

		/* sleep waiting for data, stopping to check for PPM
		 * input at 50Hz */
		int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);

		/* this would be bad... */
		if (ret < 0) {
			log("poll error %d", errno);
			continue;

		} else if (ret == 0) {
			/* timeout: no control data, switch to failsafe values */
//			warnx("no PWM: failsafe");

		} else {

			/* get controls for required topics */
			unsigned poll_id = 0;
			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
				if (_control_subs[i] > 0) {
					if (_poll_fds[poll_id].revents & POLLIN) {
						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
					}
					poll_id++;
				}
			}

			/* can we mix? */
			if (_mixers != nullptr) {

				unsigned num_outputs;

				switch (_mode) {
				case MODE_2PWM:
					num_outputs = 2;
					break;

				case MODE_4PWM:
					num_outputs = 4;
					break;

				case MODE_6PWM:
					num_outputs = 6;
					break;

				case MODE_8PWM:
					num_outputs = 8;
					break;
				default:
					num_outputs = 0;
					break;
				}

				/* do mixing */
				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
				outputs.timestamp = hrt_absolute_time();

				/* iterate actuators */
				for (unsigned i = 0; i < num_outputs; i++) {
					/* last resort: catch NaN and INF */
					if ((i >= outputs.noutputs) ||
						!isfinite(outputs.output[i])) {
						/*
						 * Value is NaN, INF or out of band - set to the minimum value.
						 * This will be clearly visible on the servo status and will limit the risk of accidentally
						 * spinning motors. It would be deadly in flight.
						 */
						outputs.output[i] = -1.0f;
					}
				}

				uint16_t pwm_limited[num_outputs];

				/* the PWM limit call takes care of out of band errors and constrains */
				pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);

				/* output to the servos */
				for (unsigned i = 0; i < num_outputs; i++) {
					up_pwm_servo_set(i, pwm_limited[i]);
				}

				/* publish mixed control outputs */
				if (_outputs_pub != nullptr) {
					_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
				} else {

					orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
				}
			}
		}

		/* check arming state */
		bool updated = false;
		orb_check(_armed_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);

			/* update the armed status and check that we're not locked down */
			bool set_armed = _armed.armed && !_armed.lockdown;

			if (_servo_armed != set_armed)
				_servo_armed = set_armed;

			/* update PWM status if armed or if disarmed PWM values are set */
			bool pwm_on = (_armed.armed || _num_disarmed_set > 0);

			if (_pwm_on != pwm_on) {
				_pwm_on = pwm_on;
				up_pwm_servo_arm(pwm_on);
			}
		}

		orb_check(_param_sub, &updated);
		if (updated) {
			parameter_update_s pupdate;
			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);

			update_pwm_rev_mask();
		}

#ifdef HRT_PPM_CHANNEL

		// see if we have new PPM input data
		if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
			// we have a new PPM frame. Publish it.
			rc_in.channel_count = ppm_decoded_channels;

			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
			}

			for (uint8_t i = 0; i < rc_in.channel_count; i++) {
				rc_in.values[i] = ppm_buffer[i];
			}

			rc_in.timestamp_publication = ppm_last_valid_decode;
			rc_in.timestamp_last_signal = ppm_last_valid_decode;

			rc_in.rc_ppm_frame_length = ppm_frame_length;
			rc_in.rssi = RC_INPUT_RSSI_MAX;
			rc_in.rc_failsafe = false;
			rc_in.rc_lost = false;
			rc_in.rc_lost_frame_count = 0;
			rc_in.rc_total_frame_count = 0;

			/* lazily advertise on first publication */
			if (to_input_rc == 0) {
				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);

			} else {
				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
			}
		}

#endif

	}

	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
		if (_control_subs[i] > 0) {
			::close(_control_subs[i]);
			_control_subs[i] = -1;
		}
	}
	::close(_armed_sub);
	::close(_param_sub);

	/* make sure servos are off */
	up_pwm_servo_deinit();

	log("stopping");

	/* note - someone else is responsible for restoring the GPIO config */

	/* tell the dtor that we are exiting */
	_task = -1;
	_exit(0);
}
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
{
	calibrate_return result = calibrate_return_ok;

	mag_worker_data_t worker_data;

	worker_data.mavlink_log_pub = mavlink_log_pub;
	worker_data.done_count = 0;
	worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
	worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;

	// Collect: Right-side up, Left Side, Nose down
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;

	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
	usleep(100000);

	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		// Initialize to no subscription
		worker_data.sub_mag[cur_mag] = -1;

		// Initialize to no memory allocated
		worker_data.x[cur_mag] = NULL;
		worker_data.y[cur_mag] = NULL;
		worker_data.z[cur_mag] = NULL;
		worker_data.calibration_counter_total[cur_mag] = 0;
	}

	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;

	char str[30];

	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
			calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
			result = calibrate_return_error;
		}
	}


	// Setup subscriptions to mag sensors
	if (result == calibrate_return_ok) {

		// We should not try to subscribe if the topic doesn't actually exist and can be counted.
		const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));

		for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
			// Mag in this slot is available
			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);

#ifdef __PX4_QURT
			// For QURT respectively the driver framework, we need to get the device ID by copying one report.
			struct mag_report	mag_report;
			orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
			device_ids[cur_mag] = mag_report.device_id;
#endif
			if (worker_data.sub_mag[cur_mag] < 0) {
				calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
				result = calibrate_return_error;
				break;
			}

			if (device_ids[cur_mag] != 0) {
				// Get priority
				int32_t prio;
				orb_priority(worker_data.sub_mag[cur_mag], &prio);

				if (prio > device_prio_max) {
					device_prio_max = prio;
					device_id_primary = device_ids[cur_mag];
				}
			} else {
				calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
				result = calibrate_return_error;
				break;
			}
		}
	}

	// Limit update rate to get equally spaced measurements over time (in ms)
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available
				unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;

				//calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
				orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
			}
		}

	}

	if (result == calibrate_return_ok) {
		int cancel_sub  = calibrate_cancel_subscribe();

		result = calibrate_from_orientation(mavlink_log_pub,                    // uORB handle to write output
						    cancel_sub,                         // Subscription to vehicle_command for cancel support
						    worker_data.side_data_collected,    // Sides to calibrate
						    mag_calibration_worker,             // Calibration worker
						    &worker_data,			// Opaque data for calibration worked
						    true);				// true: lenient still detection
		calibrate_cancel_unsubscribe(cancel_sub);
	}

	// Close subscriptions
	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
		if (worker_data.sub_mag[cur_mag] >= 0) {
			px4_close(worker_data.sub_mag[cur_mag]);
		}
	}

	// Calculate calibration values for each mag


	float sphere_x[max_mags];
	float sphere_y[max_mags];
	float sphere_z[max_mags];
	float sphere_radius[max_mags];

	// Sphere fit the data to get calibration values
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available and we should have values for it to calibrate

				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
							 worker_data.calibration_counter_total[cur_mag],
							 100, 0.0f,
							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
							 &sphere_radius[cur_mag]);

				if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
					calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
					result = calibrate_return_error;
				}

				if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
					fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
					fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
					calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
					calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
						(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
					result = calibrate_return_ok;
				}
			}
		}
	}

	// Print uncalibrated data points
	if (result == calibrate_return_ok) {

		// DO NOT REMOVE! Critical validation data!

		// printf("RAW DATA:\n--------------------\n");
		// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

		// 	if (worker_data.calibration_counter_total[cur_mag] == 0) {
		// 		continue;
		// 	}

		// 	printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

		// 	for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
		// 		float x = worker_data.x[cur_mag][i];
		// 		float y = worker_data.y[cur_mag][i];
		// 		float z = worker_data.z[cur_mag][i];
		// 		printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
		// 	}

		// 	printf(">>>>>>>\n");
		// }

		// printf("CALIBRATED DATA:\n--------------------\n");
		// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

		// 	if (worker_data.calibration_counter_total[cur_mag] == 0) {
		// 		continue;
		// 	}

		// 	printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

		// 	for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
		// 		float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
		// 		float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
		// 		float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
		// 		printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
		// 	}

		// 	printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
		// 	printf(">>>>>>>\n");
		// }
	}

	// Data points are no longer needed
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		free(worker_data.x[cur_mag]);
		free(worker_data.y[cur_mag]);
		free(worker_data.z[cur_mag]);
	}

	if (result == calibrate_return_ok) {

		(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));

		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				struct mag_calibration_s mscale;
#ifndef __PX4_QURT
				int fd_mag = -1;

				// Set new scale
				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
				fd_mag = px4_open(str, 0);
				if (fd_mag < 0) {
					calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
					result = calibrate_return_error;
				}

				if (result == calibrate_return_ok) {
					if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
						calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
						result = calibrate_return_error;
					}
				}
#endif

				if (result == calibrate_return_ok) {
					mscale.x_offset = sphere_x[cur_mag];
					mscale.y_offset = sphere_y[cur_mag];
					mscale.z_offset = sphere_z[cur_mag];

#ifndef __PX4_QURT
					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
						calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
						result = calibrate_return_error;
					}
#endif
				}

#ifndef __PX4_QURT
				// Mag device no longer needed
				if (fd_mag >= 0) {
					px4_close(fd_mag);
				}
#endif

				if (result == calibrate_return_ok) {
					bool failed = false;

					/* set parameters */

					(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);

					// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
#endif

					if (failed) {
						calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
						result = calibrate_return_error;
					} else {
						calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
									     cur_mag,
									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
						calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
									     cur_mag,
									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
						usleep(200000);
					}
				}
			}
		}
	}

	return result;
}
void
FixedwingAttitudeControl::task_main()
{

    /* inform about start */
    warnx("Initializing..");
    fflush(stdout);

    /*
     * do subscriptions
     */
    _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
    _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
    _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
    _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
    _params_sub = orb_subscribe(ORB_ID(parameter_update));
    _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
    _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));

    /* rate limit vehicle status updates to 5Hz */
    orb_set_interval(_vcontrol_mode_sub, 200);
    /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
    orb_set_interval(_att_sub, 17);

    parameters_update();

    /* get an initial update for all sensor and status data */
    vehicle_airspeed_poll();
    vehicle_setpoint_poll();
    vehicle_accel_poll();
    vehicle_control_mode_poll();
    vehicle_manual_poll();

    /* wakeup source(s) */
    struct pollfd fds[2];

    /* Setup of loop */
    fds[0].fd = _params_sub;
    fds[0].events = POLLIN;
    fds[1].fd = _att_sub;
    fds[1].events = POLLIN;

    while (!_task_should_exit) {

        static int loop_counter = 0;

        /* wait for up to 500ms for data */
        int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

        /* timed out - periodic check for _task_should_exit, etc. */
        if (pret == 0)
            continue;

        /* this is undesirable but not much we can do - might want to flag unhappy status */
        if (pret < 0) {
            warn("poll error %d, %d", pret, errno);
            continue;
        }

        perf_begin(_loop_perf);

        /* only update parameters if they changed */
        if (fds[0].revents & POLLIN) {
            /* read from param to clear updated flag */
            struct parameter_update_s update;
            orb_copy(ORB_ID(parameter_update), _params_sub, &update);

            /* update parameters from storage */
            parameters_update();
        }

        /* only run controller if attitude changed */
        if (fds[1].revents & POLLIN) {


            static uint64_t last_run = 0;
            float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
            last_run = hrt_absolute_time();

            /* guard against too large deltaT's */
            if (deltaT > 1.0f)
                deltaT = 0.01f;

            /* load local copies */
            orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);

            vehicle_airspeed_poll();

            vehicle_setpoint_poll();

            vehicle_accel_poll();

            vehicle_control_mode_poll();

            vehicle_manual_poll();

            global_pos_poll();

            /* lock integrator until control is started */
            bool lock_integrator;

            if (_vcontrol_mode.flag_control_attitude_enabled) {
                lock_integrator = false;

            } else {
                lock_integrator = true;
            }

            /* Simple handling of failsafe: deploy parachute if failsafe is on */
            if (_vcontrol_mode.flag_control_termination_enabled) {
                _actuators_airframe.control[1] = 1.0f;
//				warnx("_actuators_airframe.control[1] = 1.0f;");
            } else {
                _actuators_airframe.control[1] = 0.0f;
//				warnx("_actuators_airframe.control[1] = -1.0f;");
            }

            /* decide if in stabilized or full manual control */

            if (_vcontrol_mode.flag_control_attitude_enabled) {

                /* scale around tuning airspeed */

                float airspeed;

                /* if airspeed is not updating, we assume the normal average speed */
                if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
                                     hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
                    airspeed = _parameters.airspeed_trim;
                    if (nonfinite) {
                        perf_count(_nonfinite_input_perf);
                    }
                } else {
                    airspeed = _airspeed.true_airspeed_m_s;
                }

                /*
                 * For scaling our actuators using anything less than the min (close to stall)
                 * speed doesn't make any sense - its the strongest reasonable deflection we
                 * want to do in flight and its the baseline a human pilot would choose.
                 *
                 * Forcing the scaling to this value allows reasonable handheld tests.
                 */

                float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);

                float roll_sp = _parameters.rollsp_offset_rad;
                float pitch_sp = _parameters.pitchsp_offset_rad;
                float throttle_sp = 0.0f;

                if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
                    /* read in attitude setpoint from attitude setpoint uorb topic */
                    roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
                    pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
                    throttle_sp = _att_sp.thrust;

                    /* reset integrals where needed */
                    if (_att_sp.roll_reset_integral) {
                        _roll_ctrl.reset_integrator();
                    }
                    if (_att_sp.pitch_reset_integral) {
                        _pitch_ctrl.reset_integrator();
                    }
                    if (_att_sp.yaw_reset_integral) {
                        _yaw_ctrl.reset_integrator();
                    }
                } else {
                    /*
                     * Scale down roll and pitch as the setpoints are radians
                     * and a typical remote can only do around 45 degrees, the mapping is
                     * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
                     *
                     * With this mapping the stick angle is a 1:1 representation of
                     * the commanded attitude.
                     *
                     * The trim gets subtracted here from the manual setpoint to get
                     * the intended attitude setpoint. Later, after the rate control step the
                     * trim is added again to the control signal.
                     */
                    roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
                              + _parameters.rollsp_offset_rad;
                    pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
                               + _parameters.pitchsp_offset_rad;
                    throttle_sp = _manual.z;
                    _actuators.control[4] = _manual.flaps;

                    /*
                     * in manual mode no external source should / does emit attitude setpoints.
                     * emit the manual setpoint here to allow attitude controller tuning
                     * in attitude control mode.
                     */
                    struct vehicle_attitude_setpoint_s att_sp;
                    att_sp.timestamp = hrt_absolute_time();
                    att_sp.roll_body = roll_sp;
                    att_sp.pitch_body = pitch_sp;
                    att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
                    att_sp.thrust = throttle_sp;

                    /* lazily publish the setpoint only once available */
                    if (_attitude_sp_pub > 0) {
                        /* publish the attitude setpoint */
                        orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);

                    } else {
                        /* advertise and publish */
                        _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
                    }
                }

                /* Prepare speed_body_u and speed_body_w */
                float speed_body_u = 0.0f;
                float speed_body_v = 0.0f;
                float speed_body_w = 0.0f;
                if(_att.R_valid) 	{
                    speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
                    speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
                    speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
                } else	{
                    if (loop_counter % 10 == 0) {
                        warnx("Did not get a valid R\n");
                    }
                }

                /* Run attitude controllers */
                if (isfinite(roll_sp) && isfinite(pitch_sp)) {
                    _roll_ctrl.control_attitude(roll_sp, _att.roll);
                    _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
                    _yaw_ctrl.control_attitude(_att.roll, _att.pitch,
                                               speed_body_u, speed_body_v, speed_body_w,
                                               _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude

                    /* Run attitude RATE controllers which need the desired attitudes from above, add trim */
                    float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
                                   _att.rollspeed, _att.yawspeed,
                                   _yaw_ctrl.get_desired_rate(),
                                   _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
                    _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
                    if (!isfinite(roll_u)) {
                        _roll_ctrl.reset_integrator();
                        perf_count(_nonfinite_output_perf);

                        if (loop_counter % 10 == 0) {
                            warnx("roll_u %.4f", (double)roll_u);
                        }
                    }

                    float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
                                    _att.pitchspeed, _att.yawspeed,
                                    _yaw_ctrl.get_desired_rate(),
                                    _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
                    _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
                    if (!isfinite(pitch_u)) {
                        _pitch_ctrl.reset_integrator();
                        perf_count(_nonfinite_output_perf);
                        if (loop_counter % 10 == 0) {
                            warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
                                  " airspeed %.4f, airspeed_scaling %.4f,"
                                  " roll_sp %.4f, pitch_sp %.4f,"
                                  " _roll_ctrl.get_desired_rate() %.4f,"
                                  " _pitch_ctrl.get_desired_rate() %.4f"
                                  " att_sp.roll_body %.4f",
                                  (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
                                  (double)airspeed, (double)airspeed_scaling,
                                  (double)roll_sp, (double)pitch_sp,
                                  (double)_roll_ctrl.get_desired_rate(),
                                  (double)_pitch_ctrl.get_desired_rate(),
                                  (double)_att_sp.roll_body);
                        }
                    }

                    float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
                                  _att.pitchspeed, _att.yawspeed,
                                  _pitch_ctrl.get_desired_rate(),
                                  _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
                    _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
                    if (!isfinite(yaw_u)) {
                        _yaw_ctrl.reset_integrator();
                        perf_count(_nonfinite_output_perf);
                        if (loop_counter % 10 == 0) {
                            warnx("yaw_u %.4f", (double)yaw_u);
                        }
                    }

                    /* throttle passed through */
                    _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
                    if (!isfinite(throttle_sp)) {
                        if (loop_counter % 10 == 0) {
                            warnx("throttle_sp %.4f", (double)throttle_sp);
                        }
                    }
                } else {
                    perf_count(_nonfinite_input_perf);
                    if (loop_counter % 10 == 0) {
                        warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
                    }
                }

                /*
                 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
                 * only once available
                 */
                vehicle_rates_setpoint_s rates_sp;
                rates_sp.roll = _roll_ctrl.get_desired_rate();
                rates_sp.pitch = _pitch_ctrl.get_desired_rate();
                rates_sp.yaw = _yaw_ctrl.get_desired_rate();

                rates_sp.timestamp = hrt_absolute_time();

                if (_rate_sp_pub > 0) {
                    /* publish the attitude setpoint */
                    orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);

                } else {
                    /* advertise and publish */
                    _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
                }

            } else {
                /* manual/direct control */
                _actuators.control[0] = _manual.y;
                _actuators.control[1] = -_manual.x;
                _actuators.control[2] = _manual.r;
                _actuators.control[3] = _manual.z;
                _actuators.control[4] = _manual.flaps;
            }

            _actuators.control[5] = _manual.aux1;
            _actuators.control[6] = _manual.aux2;
            _actuators.control[7] = _manual.aux3;

            /* lazily publish the setpoint only once available */
            _actuators.timestamp = hrt_absolute_time();
            _actuators_airframe.timestamp = hrt_absolute_time();

            if (_actuators_0_pub > 0) {
                /* publish the attitude setpoint */
                orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);

            } else {
                /* advertise and publish */
                _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
            }

            if (_actuators_1_pub > 0) {
                /* publish the attitude setpoint */
                orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
//				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
//						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
//						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
//						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);

            } else {
                /* advertise and publish */
                _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
            }

        }

        loop_counter++;
        perf_end(_loop_perf);
    }

    warnx("exiting.\n");

    _control_task = -1;
    _exit(0);
}
Exemple #30
0
void
Sensors::task_main()
{

	/* start individual sensors */
	accel_init();
	gyro_init();
	mag_init();
	baro_init();
	adc_init();

	/*
	 * do subscriptions
	 */
	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
	_rc_sub = orb_subscribe(ORB_ID(input_rc));
	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vcontrol_mode_sub, 200);

	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
	orb_set_interval(_gyro_sub, 4);

	/*
	 * do advertisements
	 */
	struct sensor_combined_s raw;
	memset(&raw, 0, sizeof(raw));
	raw.timestamp = hrt_absolute_time();
	raw.adc_voltage_v[0] = 0.0f;
	raw.adc_voltage_v[1] = 0.0f;
	raw.adc_voltage_v[2] = 0.0f;
	raw.adc_voltage_v[3] = 0.0f;

	memset(&_battery_status, 0, sizeof(_battery_status));
	_battery_status.voltage_v = -1.0f;
	_battery_status.voltage_filtered_v = -1.0f;
	_battery_status.current_a = -1.0f;
	_battery_status.discharged_mah = -1.0f;

	/* get a set of initial values */
	accel_poll(raw);
	gyro_poll(raw);
	mag_poll(raw);
	baro_poll(raw);
	diff_pres_poll(raw);

	parameter_update_poll(true /* forced */);

	/* advertise the sensor_combined topic and make the initial publication */
	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);

	/* wakeup source(s) */
	struct pollfd fds[1];

	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
	fds[0].fd = _gyro_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 50ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);

		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		/* check vehicle status for changes to publication state */
		vehicle_control_mode_poll();

		/* check parameters for updates */
		parameter_update_poll();

		/* the timestamp of the raw struct is updated by the gyro_poll() method */

		/* copy most recent sensor data */
		gyro_poll(raw);
		accel_poll(raw);
		mag_poll(raw);
		baro_poll(raw);

		/* check battery voltage */
		adc_poll(raw);

		diff_pres_poll(raw);

		/* Inform other processes that new data is available to copy */
		if (_publishing) {
			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
		}

		/* Look for new r/c input data */
		rc_poll();

		perf_end(_loop_perf);
	}

	warnx("[sensors] exiting.");

	_sensors_task = -1;
	_exit(0);
}