コード例 #1
0
static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
	DevHandle h;
	DevMgr::getHandle(s, h);

	if (!h.isValid()) {
		if (!optional) {
			if (report_fail) {
				mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
			}
		}

		return false;
	}

	device_id = -1000;

	// TODO: There is no baro calibration yet, since no external baros exist
	// int ret = check_calibration(fd, "CAL_BARO%u_ID");

	// if (ret) {
	// 	mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
	// 	success = false;
	// 	goto out;
	// }

//out:

	DevMgr::releaseHandle(h);
	return success;
}
コード例 #2
0
ファイル: PreflightCheck.cpp プロジェクト: JW-CHOI/Firmware
static bool gnssCheck(int mavlink_fd)
{
	bool success = true;

	int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));

	//Wait up to 2000ms to allow the driver to detect a GNSS receiver module
	px4_pollfd_struct_t fds[1];
	fds[0].fd = gpsSub;
	fds[0].events = POLLIN;
	if(px4_poll(fds, 1, 2000) <= 0) {
		success = false;
	}
	else {
		struct vehicle_gps_position_s gps;
		if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
		    (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {
			success = false;
		}
	}

	//Report failure to detect module
	if(!success) {
		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
	}

	px4_close(gpsSub);
	return success;
}
コード例 #3
0
ファイル: flow.cpp プロジェクト: ButterZone/Firmware
void BlockLocalPositionEstimator::flowCheckTimeout()
{
	if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
		if (_flowInitialized) {
			flowDeinit();
			mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
		}
	}
}
コード例 #4
0
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_MAG%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, MAGIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}
コード例 #5
0
ファイル: vision.cpp プロジェクト: 2013-8-15/Firmware
void BlockLocalPositionEstimator::visionCheckTimeout()
{
	if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
		if (_visionInitialized) {
			_visionInitialized = false;
			_visionStats.reset();
			mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] vision position timeout ");
		}
	}
}
コード例 #6
0
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
	DevHandle h;
	DevMgr::getHandle(s, h);

	if (!h.isValid()) {
		if (!optional) {
			if (report_fail) {
				mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
			}
		}

		return false;
	}

	int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);

	if (ret) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
		}
		success = false;
		goto out;
	}

	ret = h.ioctl(GYROIOCSELFTEST, 0);

	if (ret != OK) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
		}
		success = false;
		goto out;
	}

out:
	DevMgr::releaseHandle(h);
	return success;
}
コード例 #7
0
ファイル: mission.cpp プロジェクト: JGSOpenSrc/Firmware
void
Mission::reset_offboard_mission(struct mission_s &mission)
{
	dm_lock(DM_KEY_MISSION_STATE);

	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
			/* set current item to 0 */
			mission.current_seq = 0;

			/* reset jump counters */
			if (mission.count > 0) {
				dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);

				for (unsigned index = 0; index < mission.count; index++) {
					struct mission_item_s item;
					const ssize_t len = sizeof(struct mission_item_s);

					if (dm_read(dm_current, index, &item, len) != len) {
						PX4_WARN("could not read mission item during reset");
						break;
					}

					if (item.nav_cmd == NAV_CMD_DO_JUMP) {
						item.do_jump_current_count = 0;

						if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET,
							     &item, len) != len) {
							PX4_WARN("could not save mission item during reset");
							break;
						}
					}
				}
			}

		} else {
			mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: could not read mission");

			/* initialize mission state in dataman */
			mission.dataman_id = 0;
			mission.count = 0;
			mission.current_seq = 0;
		}

		dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
	}

	dm_unlock(DM_KEY_MISSION_STATE);
}
コード例 #8
0
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail)
{
	bool success = true;
	int ret;
	int fd = orb_subscribe(ORB_ID(airspeed));

	struct airspeed_s airspeed;

	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
		}
		success = false;
		goto out;
	}

	if (fabsf(airspeed.confidence) < 0.99f) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");
		}
		success = false;
		goto out;
	}

	if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
		}
		// XXX do not make this fatal yet
	}

out:
	px4_close(fd);
	return success;
}
コード例 #9
0
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
		}

		return false;
	}

	close(fd);
	return success;
}
コード例 #10
0
int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report)
{
	/* 
	 */
	bool reportFailures = force_report || (!status->data_link_lost &&
		!status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout);
	
	bool checkAirspeed = false;
	/* Perform airspeed check only if circuit breaker is not
	 * engaged and it's not a rotary wing */
	if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
		checkAirspeed = true;
	}

	bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
		
	if (!status->cb_usb && status->usb_connected && prearm) {
		preflight_ok = false;
		if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
	}

	return !preflight_ok;
}
コード例 #11
0
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed)
{
	int	return_code = OK;

	int	fd = -1;

	struct	battery_status_s battery;
	int	batt_sub = -1;
	bool	batt_updated = false;
	bool	batt_connected = false;

	hrt_abstime battery_connect_wait_timeout = 30000000;
	hrt_abstime pwm_high_timeout = 10000000;
	hrt_abstime timeout_start;

	mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");

	batt_sub = orb_subscribe(ORB_ID(battery_status));
	if (batt_sub < 0) {
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
		goto Error;
	}

	// Make sure battery is disconnected
	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
	if (battery.voltage_filtered_v > 3.0f) {
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
		goto Error;
	}

	armed->in_esc_calibration_mode = true;

	fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);

	if (fd < 0) {
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
		goto Error;
	}

	/* tell IO/FMU that its ok to disable its safety with the switch */
	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
		goto Error;
	}

	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
		goto Error;
	}

	/* tell IO to switch off safety without using the safety switch */
	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
		goto Error;
	}

	mavlink_and_console_log_info(mavlink_log_pub, "[cal] Connect battery now");

	timeout_start = hrt_absolute_time();

	while (true) {
		// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
		// sit high.
		hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;

		if (hrt_absolute_time() - timeout_start > timeout_wait) {
			if (!batt_connected) {
				mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
				goto Error;
			}

			// PWM was high long enough
			break;
		}

		if (!batt_connected) {
			orb_check(batt_sub, &batt_updated);
			if (batt_updated) {
				orb_copy(ORB_ID(battery_status), batt_sub, &battery);
				if (battery.voltage_filtered_v > 3.0f) {
					// Battery is connected, signal to user and start waiting again
					batt_connected = true;
					timeout_start = hrt_absolute_time();
					mavlink_and_console_log_info(mavlink_log_pub, "[cal] Battery connected");
				}
			}
		}
		usleep(50000);
	}

Out:
	if (batt_sub != -1) {
		orb_unsubscribe(batt_sub);
	}
	if (fd != -1) {
		if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
			mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off");
		}
		if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
			mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed");
		}
		if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
			mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
		}
		px4_close(fd);
	}
	armed->in_esc_calibration_mode = false;

	if (return_code == OK) {
		mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
	}

	return return_code;

Error:
	return_code = ERROR;
	goto Out;
}
コード例 #12
0
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors)
{
	const unsigned samples_num = 3000;
	*active_sensors = 0;

	float accel_ref[max_sens][6][3];
	bool data_collected[6] = { false, false, false, false, false, false };
	const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };

	int subs[max_sens];

	uint64_t timestamps[max_sens];

	for (unsigned i = 0; i < max_sens; i++) {
		subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
		/* store initial timestamp - used to infer which sensors are active */
		struct accel_report arp = {};
		(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
		timestamps[i] = arp.timestamp;
	}

	unsigned done_count = 0;
	int res = OK;

	while (true) {
		bool done = true;
		unsigned old_done_count = done_count;
		done_count = 0;

		for (int i = 0; i < 6; i++) {
			if (data_collected[i]) {
				done_count++;

			} else {
				done = false;
			}
		}

		if (old_done_count != done_count) {
			mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
		}

		if (done) {
			break;
		}

		/* inform user which axes are still needed */
		mavlink_and_console_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
				 (!data_collected[5]) ? "down " : "",
				 (!data_collected[0]) ? "back " : "",
				 (!data_collected[1]) ? "front " : "",
				 (!data_collected[2]) ? "left " : "",
				 (!data_collected[3]) ? "right " : "",
				 (!data_collected[4]) ? "up " : "");

		/* allow user enough time to read the message */
		sleep(3);

		int orient = detect_orientation(mavlink_fd, subs);

		if (orient < 0) {
			mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
			sleep(3);
			continue;
		}

		/* inform user about already handled side */
		if (data_collected[orient]) {
			mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
			sleep(3);
			continue;
		}

		mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
		sleep(1);
		read_accelerometer_avg(subs, accel_ref, orient, samples_num);
		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
				 (double)accel_ref[0][orient][0],
				 (double)accel_ref[0][orient][1],
				 (double)accel_ref[0][orient][2]);

		data_collected[orient] = true;
		tune_neutral(true);
	}

	/* close all subscriptions */
	for (unsigned i = 0; i < max_sens; i++) {
		/* figure out which sensors were active */
		struct accel_report arp = {};
		(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
		if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
			(*active_sensors)++;
		}
		close(subs[i]);
	}

	if (res == OK) {
		/* calculate offsets and transform matrix */
		for (unsigned i = 0; i < (*active_sensors); i++) {
			res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);

			/* verbose output on the console */
			printf("accel_T transformation matrix:\n");
			for (unsigned j = 0; j < 3; j++) {
				printf("  %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]);
			}
			printf("\n");

			if (res != OK) {
				mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
				break;
			}
		}
	}

	return res;
}
コード例 #13
0
/**
 * Wait for vehicle become still and detect it's orientation.
 *
 * @param mavlink_fd the MAVLink file descriptor to print output to
 * @param subs the accelerometer subscriptions. Only the first one will be used.
 *
 * @return 0..5 according to orientation when vehicle is still and ready for measurements,
 * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
 */
int detect_orientation(int mavlink_fd, int (&subs)[max_sens])
{
	const unsigned ndim = 3;

	struct accel_report sensor;
	/* exponential moving average of accel */
	float accel_ema[ndim] = { 0.0f };
	/* max-hold dispersion of accel */
	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
	/* EMA time constant in seconds*/
	float ema_len = 0.5f;
	/* set "still" threshold to 0.25 m/s^2 */
	float still_thr2 = powf(0.25f, 2);
	/* set accel error threshold to 5m/s^2 */
	float accel_err_thr = 5.0f;
	/* still time required in us */
	hrt_abstime still_time = 2000000;
	struct pollfd fds[1];
	fds[0].fd = subs[0];
	fds[0].events = POLLIN;

	hrt_abstime t_start = hrt_absolute_time();
	/* set timeout to 30s */
	hrt_abstime timeout = 30000000;
	hrt_abstime t_timeout = t_start + timeout;
	hrt_abstime t = t_start;
	hrt_abstime t_prev = t_start;
	hrt_abstime t_still = 0;

	unsigned poll_errcount = 0;

	while (true) {
		/* wait blocking for new data */
		int poll_ret = poll(fds, 1, 1000);

		if (poll_ret) {
			orb_copy(ORB_ID(sensor_accel), subs[0], &sensor);
			t = hrt_absolute_time();
			float dt = (t - t_prev) / 1000000.0f;
			t_prev = t;
			float w = dt / ema_len;

			for (unsigned i = 0; i < ndim; i++) {

				float di = 0.0f;
				switch (i) {
					case 0:
						di = sensor.x;
						break;
					case 1:
						di = sensor.y;
						break;
					case 2:
						di = sensor.z;
						break;
				}

				float d = di - accel_ema[i];
				accel_ema[i] += d * w;
				d = d * d;
				accel_disp[i] = accel_disp[i] * (1.0f - w);

				if (d > still_thr2 * 8.0f) {
					d = still_thr2 * 8.0f;
				}

				if (d > accel_disp[i]) {
					accel_disp[i] = d;
				}
			}

			/* still detector with hysteresis */
			if (accel_disp[0] < still_thr2 &&
			    accel_disp[1] < still_thr2 &&
			    accel_disp[2] < still_thr2) {
				/* is still now */
				if (t_still == 0) {
					/* first time */
					mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
					t_still = t;
					t_timeout = t + timeout;

				} else {
					/* still since t_still */
					if (t > t_still + still_time) {
						/* vehicle is still, exit from the loop to detection of its orientation */
						break;
					}
				}

			} else if (accel_disp[0] > still_thr2 * 4.0f ||
				   accel_disp[1] > still_thr2 * 4.0f ||
				   accel_disp[2] > still_thr2 * 4.0f) {
				/* not still, reset still start time */
				if (t_still != 0) {
					mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
					sleep(3);
					t_still = 0;
				}
			}

		} else if (poll_ret == 0) {
			poll_errcount++;
		}

		if (t > t_timeout) {
			poll_errcount++;
		}

		if (poll_errcount > 1000) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
			return ERROR;
		}
	}

	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 0;        // [ g, 0, 0 ]
	}

	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 1;        // [ -g, 0, 0 ]
	}

	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 2;        // [ 0, g, 0 ]
	}

	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 3;        // [ 0, -g, 0 ]
	}

	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
		return 4;        // [ 0, 0, g ]
	}

	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
		return 5;        // [ 0, 0, -g ]
	}

	mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");

	return ERROR;	// Can't detect orientation
}
コード例 #14
0
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
	int fd = open(s, O_RDONLY);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_ACC%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, ACCELIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

	if (dynamic) {
		/* check measurement result range */
		struct accel_report acc;
		ret = read(fd, &acc, sizeof(acc));

		if (ret == sizeof(acc)) {
			/* evaluate values */
			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);

			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
				/* this is frickin' fatal */
				success = false;
				goto out;
			}
		} else {
			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
			/* this is frickin' fatal */
			success = false;
			goto out;
		}
	}

out:
	close(fd);
	return success;
}
コード例 #15
0
int do_accel_calibration(int mavlink_fd)
{
	int fd;
	int32_t device_id[max_sens];

	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);

	mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides");
	sleep(3);
	mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen");
	sleep(5);

	struct accel_scale accel_scale = {
		0.0f,
		1.0f,
		0.0f,
		1.0f,
		0.0f,
		1.0f,
	};

	int res = OK;

	char str[30];

	/* reset all sensors */
	for (unsigned s = 0; s < max_sens; s++) {
		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
		/* reset all offsets to zero and all scales to one */
		fd = open(str, 0);

		if (fd < 0) {
			continue;
		}

		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);

		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
		close(fd);

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
		}
	}

	float accel_offs[max_sens][3];
	float accel_T[max_sens][3][3];
	unsigned active_sensors;

	if (res == OK) {
		/* measure and calculate offsets & scales */
		res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
	}

	if (res != OK || active_sensors == 0) {
		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
		return ERROR;
	}

	/* measurements completed successfully, rotate calibration values */
	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
	int32_t board_rotation_int;
	param_get(board_rotation_h, &(board_rotation_int));
	enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
	math::Matrix<3, 3> board_rotation;
	get_rot_matrix(board_rotation_id, &board_rotation);
	math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();

	for (unsigned i = 0; i < active_sensors; i++) {

		/* handle individual sensors, one by one */
		math::Vector<3> accel_offs_vec(accel_offs[i]);
		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
		math::Matrix<3, 3> accel_T_mat(accel_T[i]);
		math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;

		accel_scale.x_offset = accel_offs_rotated(0);
		accel_scale.x_scale = accel_T_rotated(0, 0);
		accel_scale.y_offset = accel_offs_rotated(1);
		accel_scale.y_scale = accel_T_rotated(1, 1);
		accel_scale.z_offset = accel_offs_rotated(2);
		accel_scale.z_scale = accel_T_rotated(2, 2);

		bool failed = false;

		/* set parameters */
		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
		failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset)));
		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
		failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset)));
		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
		failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset)));
		(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
		failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale)));
		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
		failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale)));
		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
		failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale)));
		(void)sprintf(str, "CAL_ACC%u_ID", i);
		failed |= (OK != param_set(param_find(str), &(device_id[i])));
		
		if (failed) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
			return ERROR;
		}

		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
		fd = open(str, 0);

		if (fd < 0) {
			mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
			res = ERROR;
		} else {
			res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
			close(fd);
		}

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
		}
	}

	if (res == OK) {
		/* auto-save to EEPROM */
		res = param_save_default();

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
		}

		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);

	} else {
		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
	}

	return res;
}
コード例 #16
0
int do_mag_calibration(int mavlink_fd)
{
	const unsigned max_mags = 3;

	int32_t device_id[max_mags];
	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
	sleep(1);

	struct mag_scale mscale_null[max_mags] = {
	{
		0.0f,
		1.0f,
		0.0f,
		1.0f,
		0.0f,
		1.0f,
	}
	} ;

	int res = ERROR;

	char str[30];

	unsigned calibrated_ok = 0;

	for (unsigned s = 0; s < max_mags; s++) {

		/* erase old calibration */
		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
		int fd = open(str, O_RDONLY);

		if (fd < 0) {
			continue;
		}

		mavlink_and_console_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s);
		sleep(3);

		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);

		/* ensure all scale fields are initialized tha same as the first struct */
		(void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0]));

		res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]);

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
		}

		if (res == OK) {
			/* calibrate range */
			res = ioctl(fd, MAGIOCCALIBRATE, fd);

			if (res != OK) {
				mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration");
				/* this is non-fatal - mark it accordingly */
				res = OK;
			}
		}

		close(fd);

		if (res == OK) {
			res = calibrate_instance(mavlink_fd, s, device_id[s]);

			if (res == OK) {
				calibrated_ok++;
			}
		}
	}

	if (calibrated_ok) {

		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
		usleep(100000);
		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);

		/* auto-save to EEPROM */
		res = param_save_default();

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
		}
	} else {
		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
	}

	return res;
}
コード例 #17
0
ファイル: rc_check.c プロジェクト: AdyashaDash/fw_px4_sysidCL
int rc_calibration_check(int mavlink_fd)
{

	char nbuf[20];
	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
		_parameter_handles_rev, _parameter_handles_dz;

	unsigned map_fail_count = 0;

	const char *rc_map_mandatory[] = {	"RC_MAP_MODE_SW",
						/* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/
						0 /* end marker */
					 };

	unsigned j = 0;

	/* first check channel mappings */
	while (rc_map_mandatory[j] != 0) {

		param_t map_parm = param_find(rc_map_mandatory[j]);

		if (map_parm == PARAM_INVALID) {
			mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]);
			/* give system time to flush error message in case there are more */
			usleep(100000);
			map_fail_count++;
			j++;
			continue;
		}

		int32_t mapping;
		param_get(map_parm, &mapping);

		if (mapping > RC_INPUT_MAX_CHANNELS) {
			mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]);
			/* give system time to flush error message in case there are more */
			usleep(100000);
			map_fail_count++;
		}

		if (mapping == 0) {
			mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]);
			/* give system time to flush error message in case there are more */
			usleep(100000);
			map_fail_count++;
		}

		j++;
	}

	unsigned total_fail_count = 0;
	unsigned channels_failed = 0;

	for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
		/* should the channel be enabled? */
		uint8_t count = 0;

		/* initialize values to values failing the check */
		float param_min = 0.0f;
		float param_max = 0.0f;
		float param_trim = 0.0f;
		float param_rev = 0.0f;
		float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;

		/* min values */
		sprintf(nbuf, "RC%d_MIN", i + 1);
		_parameter_handles_min = param_find(nbuf);
		param_get(_parameter_handles_min, &param_min);

		/* trim values */
		sprintf(nbuf, "RC%d_TRIM", i + 1);
		_parameter_handles_trim = param_find(nbuf);
		param_get(_parameter_handles_trim, &param_trim);

		/* max values */
		sprintf(nbuf, "RC%d_MAX", i + 1);
		_parameter_handles_max = param_find(nbuf);
		param_get(_parameter_handles_max, &param_max);

		/* channel reverse */
		sprintf(nbuf, "RC%d_REV", i + 1);
		_parameter_handles_rev = param_find(nbuf);
		param_get(_parameter_handles_rev, &param_rev);

		/* channel deadzone */
		sprintf(nbuf, "RC%d_DZ", i + 1);
		_parameter_handles_dz = param_find(nbuf);
		param_get(_parameter_handles_dz, &param_dz);

		/* assert min..center..max ordering */
		if (param_min < RC_INPUT_LOWEST_MIN_US) {
			count++;
			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US);
			/* give system time to flush error message in case there are more */
			usleep(100000);
		}

		if (param_max > RC_INPUT_HIGHEST_MAX_US) {
			count++;
			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US);
			/* give system time to flush error message in case there are more */
			usleep(100000);
		}

		if (param_trim < param_min) {
			count++;
			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min);
			/* give system time to flush error message in case there are more */
			usleep(100000);
		}

		if (param_trim > param_max) {
			count++;
			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max);
			/* give system time to flush error message in case there are more */
			usleep(100000);
		}

		/* assert deadzone is sane */
		if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US);
			/* give system time to flush error message in case there are more */
			usleep(100000);
			count++;
		}

		total_fail_count += count;

		if (count) {
			channels_failed++;
		}
	}

	if (channels_failed) {
		sleep(2);
		mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count,
						 (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
		usleep(100000);
	}

	return total_fail_count + map_fail_count;
}
コード例 #18
0
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
	calibrate_return result = calibrate_return_ok;

	*active_sensors = 0;

	accel_worker_data_t worker_data;

	worker_data.mavlink_log_pub = mavlink_log_pub;
	worker_data.done_count = 0;

	bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };

	// Initialize subs to error condition so we know which ones are open and which are not
	for (size_t i=0; i<max_accel_sens; i++) {
		worker_data.subs[i] = -1;
	}

	uint64_t timestamps[max_accel_sens];

	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
	const unsigned accel_count = orb_group_count(ORB_ID(sensor_accel));
	for (unsigned i = 0; i < accel_count; i++) {
		worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
		if (worker_data.subs[i] < 0) {
			result = calibrate_return_error;
			break;
		}

#ifdef __PX4_QURT
		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
		struct accel_report	accel_report;
		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
		device_id[i] = accel_report.device_id;
#endif
		/* store initial timestamp - used to infer which sensors are active */
		struct accel_report arp = {};
		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
		timestamps[i] = arp.timestamp;

		if (device_id[i] != 0) {
			// Get priority
			int32_t prio;
			orb_priority(worker_data.subs[i], &prio);

			if (prio > device_prio_max) {
				device_prio_max = prio;
				device_id_primary = device_id[i];
			}
		} else {
			mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", i);
			result = calibrate_return_error;
			break;
		}
	}

	if (result == calibrate_return_ok) {
		int cancel_sub = calibrate_cancel_subscribe();
		result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
		calibrate_cancel_unsubscribe(cancel_sub);
	}

	/* close all subscriptions */
	for (unsigned i = 0; i < max_accel_sens; i++) {
		if (worker_data.subs[i] >= 0) {
			/* figure out which sensors were active */
			struct accel_report arp = {};
			(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
			if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
				(*active_sensors)++;
			}
			px4_close(worker_data.subs[i]);
		}
	}

	if (result == calibrate_return_ok) {
		/* calculate offsets and transform matrix */
		for (unsigned i = 0; i < (*active_sensors); i++) {
			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);

			if (result != calibrate_return_ok) {
				mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error");
				break;
			}
		}
	}

	return result;
}
コード例 #19
0
int do_level_calibration(orb_advert_t *mavlink_log_pub) {
	const unsigned cal_time = 5;
	const unsigned cal_hz = 100;
	unsigned settle_time = 30;

	bool success = false;
	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));

	mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");

	param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
	param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
	param_t board_rot_handle = param_find("SENS_BOARD_ROT");

	// save old values if calibration fails
	float roll_offset_current;
	float pitch_offset_current;
	int32_t board_rot_current = 0;
	param_get(roll_offset_handle, &roll_offset_current);
	param_get(pitch_offset_handle, &pitch_offset_current);
	param_get(board_rot_handle, &board_rot_current);

	// give attitude some time to settle if there have been changes to the board rotation parameters
	if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) {
		settle_time = 0;
	}

	float zero = 0.0f;
	param_set(roll_offset_handle, &zero);
	param_set(pitch_offset_handle, &zero);

	px4_pollfd_struct_t fds[1];

	fds[0].fd = att_sub;
	fds[0].events = POLLIN;

	float roll_mean = 0.0f;
	float pitch_mean = 0.0f;
	unsigned counter = 0;

	// sleep for some time
	hrt_abstime start = hrt_absolute_time();
	while(hrt_elapsed_time(&start) < settle_time * 1000000) {
		mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
		sleep(settle_time / 10);
	}

	start = hrt_absolute_time();
	// average attitude for 5 seconds
	while(hrt_elapsed_time(&start) < cal_time * 1000000) {
		int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		if (pollret <= 0) {
			// attitude estimator is not running
			mavlink_and_console_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
			mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
			goto out;
		}

		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
		roll_mean += att.roll;
		pitch_mean += att.pitch;
		counter++;
	}

	mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);

	if (counter > (cal_time * cal_hz / 2 )) {
		roll_mean /= counter;
		pitch_mean /= counter;
	} else {
		mavlink_and_console_log_info(mavlink_log_pub, "not enough measurements taken");
		success = false;
		goto out;
	}

	if (fabsf(roll_mean) > 0.8f ) {
		mavlink_and_console_log_critical(mavlink_log_pub, "excess roll angle");
	} else if (fabsf(pitch_mean) > 0.8f ) {
		mavlink_and_console_log_critical(mavlink_log_pub, "excess pitch angle");
	} else {
		roll_mean *= (float)M_RAD_TO_DEG;
		pitch_mean *= (float)M_RAD_TO_DEG;
		param_set(roll_offset_handle, &roll_mean);
		param_set(pitch_offset_handle, &pitch_mean);
		success = true;
	}

out:
	if (success) {
		mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
		return 0;
	} else {
		// set old parameters
		param_set(roll_offset_handle, &roll_offset_current);
		param_set(pitch_offset_handle, &pitch_offset_current);
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
		return 1;
	}
}
コード例 #20
0
calibrate_return calibrate_from_orientation(int		mavlink_fd,
					    int		cancel_sub,
					    bool	side_data_collected[detect_orientation_side_count],
					    calibration_from_orientation_worker_t calibration_worker,
					    void*	worker_data,
					    bool	lenient_still_position)
{
	calibrate_return result = calibrate_return_ok;
	
	// Setup subscriptions to onboard accel sensor
	
	int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
	if (sub_accel < 0) {
		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
		return calibrate_return_error;
	}
	
	unsigned orientation_failures = 0;
	
	// Rotate through all requested orientation
	while (true) {
		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
			result = calibrate_return_cancelled;
			break;
		}
		
		if (orientation_failures > 4) {
			result = calibrate_return_error;
			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
			break;
		}
		
		unsigned int side_complete_count = 0;
		
		// Update the number of completed sides
		for (unsigned i = 0; i < detect_orientation_side_count; i++) {
			if (side_data_collected[i]) {
				side_complete_count++;
			}
		}
		
		if (side_complete_count == detect_orientation_side_count) {
			// We have completed all sides, move on
			break;
		}
		
		/* inform user which orientations are still needed */
		char pendingStr[256];
		pendingStr[0] = 0;
		
		for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
			if (!side_data_collected[cur_orientation]) {
				strcat(pendingStr, " ");
				strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
			}
		}
		mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
		
		mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
		enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
		
		if (orient == DETECT_ORIENTATION_ERROR) {
			orientation_failures++;
			mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
			continue;
		}
		
		/* inform user about already handled side */
		if (side_data_collected[orient]) {
			orientation_failures++;
			mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
			mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
			continue;
		}
		
		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
		orientation_failures = 0;
		
		// Call worker routine
		result = calibration_worker(orient, cancel_sub, worker_data);
		if (result != calibrate_return_ok ) {
			break;
		}
		
		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
		
		// Note that this side is complete
		side_data_collected[orient] = true;
		tune_neutral(true);
		usleep(200000);
	}
	
	if (sub_accel >= 0) {
		px4_close(sub_accel);
	}
	
	return result;
}
コード例 #21
0
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#ifndef __PX4_QURT
	int fd;
#endif

	mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);

	struct accel_calibration_s accel_scale;
	accel_scale.x_offset = 0.0f;
	accel_scale.x_scale = 1.0f;
	accel_scale.y_offset = 0.0f;
	accel_scale.y_scale = 1.0f;
	accel_scale.z_offset = 0.0f;
	accel_scale.z_scale = 1.0f;

	int res = OK;

	char str[30];

	/* reset all sensors */
	for (unsigned s = 0; s < max_accel_sens; s++) {
#ifndef __PX4_QURT
		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
		/* reset all offsets to zero and all scales to one */
		fd = px4_open(str, 0);

		if (fd < 0) {
			continue;
		}

		device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);

		res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
		px4_close(fd);

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
		}
#else
		(void)sprintf(str, "CAL_ACC%u_XOFF", s);
		res = param_set(param_find(str), &accel_scale.x_offset);
		if (res != OK) {
			PX4_ERR("unable to reset %s", str);
		}
		(void)sprintf(str, "CAL_ACC%u_YOFF", s);
		res = param_set(param_find(str), &accel_scale.y_offset);
		if (res != OK) {
			PX4_ERR("unable to reset %s", str);
		}
		(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
		res = param_set(param_find(str), &accel_scale.z_offset);
		if (res != OK) {
			PX4_ERR("unable to reset %s", str);
		}
		(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
		res = param_set(param_find(str), &accel_scale.x_scale);
		if (res != OK) {
			PX4_ERR("unable to reset %s", str);
		}
		(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
		res = param_set(param_find(str), &accel_scale.y_scale);
		if (res != OK) {
			PX4_ERR("unable to reset %s", str);
		}
		(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
		res = param_set(param_find(str), &accel_scale.z_scale);
		if (res != OK) {
			PX4_ERR("unable to reset %s", str);
		}
#endif
	}

	float accel_offs[max_accel_sens][3];
	float accel_T[max_accel_sens][3][3];
	unsigned active_sensors;

	/* measure and calculate offsets & scales */
	if (res == OK) {
		calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
		if (cal_return == calibrate_return_cancelled) {
			// Cancel message already displayed, nothing left to do
			return ERROR;
		} else if (cal_return == calibrate_return_ok) {
			res = OK;
		} else {
			res = ERROR;
		}
	}

	if (res != OK) {
		if (active_sensors == 0) {
			mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
		}
		return ERROR;
	}

	/* measurements completed successfully, rotate calibration values */
	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
	int32_t board_rotation_int;
	param_get(board_rotation_h, &(board_rotation_int));
	enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
	math::Matrix<3, 3> board_rotation;
	get_rot_matrix(board_rotation_id, &board_rotation);
	math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();

	for (unsigned i = 0; i < active_sensors; i++) {

		/* handle individual sensors, one by one */
		math::Vector<3> accel_offs_vec(accel_offs[i]);
		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
		math::Matrix<3, 3> accel_T_mat(accel_T[i]);
		math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;

		accel_scale.x_offset = accel_offs_rotated(0);
		accel_scale.x_scale = accel_T_rotated(0, 0);
		accel_scale.y_offset = accel_offs_rotated(1);
		accel_scale.y_scale = accel_T_rotated(1, 1);
		accel_scale.z_offset = accel_offs_rotated(2);
		accel_scale.z_scale = accel_T_rotated(2, 2);

		bool failed = false;

		failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));


		PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i,
				(double)accel_scale.x_offset,
				(double)accel_scale.y_offset,
				(double)accel_scale.z_offset);
		PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", i,
				(double)accel_scale.x_scale,
				(double)accel_scale.y_scale,
				(double)accel_scale.z_scale);

		/* set parameters */
		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
		(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
		(void)sprintf(str, "CAL_ACC%u_ID", i);
		failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));

		if (failed) {
			mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, i);
			return ERROR;
		}

#ifndef __PX4_QURT
		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
		fd = px4_open(str, 0);

		if (fd < 0) {
			mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
			res = ERROR;
		} else {
			res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
			px4_close(fd);
		}

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, i);
		}
#endif
	}

	if (res == OK) {
		/* auto-save to EEPROM */
		res = param_save_default();

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
		}

		/* if there is a any preflight-check system response, let the barrage of messages through */
		usleep(200000);

		mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);

	} else {
		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
	}

	/* give this message enough time to propagate */
	usleep(600000);

	return res;
}
コード例 #22
0
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;
}
コード例 #23
0
transition_result_t
arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
			const struct safety_s   *safety,		///< current safety settings
			arming_state_t          new_arming_state,	///< arming state requested
			struct actuator_armed_s *armed,			///< current armed status
			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
{
	// Double check that our static arrays are still valid
	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
	ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);

	transition_result_t ret = TRANSITION_DENIED;
	arming_state_t current_arming_state = status->arming_state;
	bool feedback_provided = false;

	/* only check transition if the new state is actually different from the current one */
	if (new_arming_state == current_arming_state) {
		ret = TRANSITION_NOT_CHANGED;

	} else {

		/*
		 * Get sensing state if necessary
		 */
		int prearm_ret = OK;

		/* only perform the check if we have to */
		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
			prearm_ret = prearm_check(status, mavlink_fd);
		}

		/*
		 * Perform an atomic state update
		 */
		#ifdef __PX4_NUTTX
		irqstate_t flags = irqsave();
		#endif

		/* enforce lockdown in HIL */
		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
			armed->lockdown = true;
			prearm_ret = OK;
			status->condition_system_sensors_initialized = true;

			/* recover from a prearm fail */
			if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
				status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
			}

		} else {
			armed->lockdown = false;
		}

		// Check that we have a valid state transition
		bool valid_transition = arming_transitions[new_arming_state][status->arming_state];

		if (valid_transition) {
			// We have a good transition. Now perform any secondary validation.
			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {

				//      Do not perform pre-arm checks if coming from in air restore
				//      Allow if vehicle_status_s::HIL_STATE_ON
				if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
					status->hil_state == vehicle_status_s::HIL_STATE_OFF) {

					// Fail transition if pre-arm check fails
					if (prearm_ret) {
						/* the prearm check already prints the reject reason */
						feedback_provided = true;
						valid_transition = false;

					// Fail transition if we need safety switch press
					} else if (safety->safety_switch_available && !safety->safety_off) {

						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
						feedback_provided = true;
						valid_transition = false;
					}

					// Perform power checks only if circuit breaker is not
					// engaged for these checks
					if (!status->circuit_breaker_engaged_power_check) {
						// Fail transition if power is not good
						if (!status->condition_power_input_valid) {

							mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
							feedback_provided = true;
							valid_transition = false;
						}

						// Fail transition if power levels on the avionics rail
						// are measured but are insufficient
						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
							// Check avionics rail voltages
							if (status->avionics_power_rail_voltage < 4.75f) {
								mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
								feedback_provided = true;
								valid_transition = false;
							} else if (status->avionics_power_rail_voltage < 4.9f) {
								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
								feedback_provided = true;
							} else if (status->avionics_power_rail_voltage > 5.4f) {
								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
								feedback_provided = true;
							}
						}
					}

				}

			} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
				new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
			}
		}

		// HIL can always go to standby
		if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
			valid_transition = true;
		}

		// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {

			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {

				if (status->condition_system_sensors_initialized) {
					mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming");
				} else {
					mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
				}
				feedback_provided = true;

			} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
					status->condition_system_sensors_initialized) {
				mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete");
				feedback_provided = true;
			} else {
				// Silent ignore
				feedback_provided = true;
			}

		// Sensors need to be initialized for STANDBY state, except for HIL
		} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
			(!status->condition_system_sensors_initialized)) {
			if (!fRunPreArmChecks) {
				mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
			}
			feedback_provided = true;
			valid_transition = false;
			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
		}

		// Finish up the state transition
		if (valid_transition) {
			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
			armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
			ret = TRANSITION_CHANGED;
			status->arming_state = new_arming_state;
		}

		/* end of atomic state update */
		#ifdef __PX4_NUTTX
		irqrestore(flags);
		#endif
	}

	if (ret == TRANSITION_DENIED) {
#define		WARNSTR "INVAL: %s - %s"
		/* only print to console here by default as this is too technical to be useful during operation */
		warnx(WARNSTR, state_names[status->arming_state], state_names[new_arming_state]);

		/* print to MAVLink if we didn't provide any feedback yet */
		if (!feedback_provided) {
			mavlink_log_critical(mavlink_fd, WARNSTR, state_names[status->arming_state], state_names[new_arming_state]);
		}
#undef WARNSTR
	}

	return ret;
}
コード例 #24
0
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
	DevHandle h;
	DevMgr::getHandle(s, h);

	if (!h.isValid()) {
		if (!optional) {
			if (report_fail) {
				mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
			}
		}

		return false;
	}

	int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);

	if (ret) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
		}
		success = false;
		goto out;
	}

	ret = h.ioctl(ACCELIOCSELFTEST, 0);

	if (ret != OK) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
		}
		success = false;
		goto out;
	}

#ifdef __PX4_NUTTX
	if (dynamic) {
		/* check measurement result range */
		struct accel_report acc;
		ret = h.read(&acc, sizeof(acc));

		if (ret == sizeof(acc)) {
			/* evaluate values */
			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);

			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
				if (report_fail) {
					mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
				}
				/* this is frickin' fatal */
				success = false;
				goto out;
			}
		} else {
			if (report_fail) {
				mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
			}
			/* this is frickin' fatal */
			success = false;
			goto out;
		}
	}
#endif

out:
	DevMgr::releaseHandle(h);
	return success;
}
コード例 #25
0
ファイル: mission.cpp プロジェクト: dolphonie/PKX4Firmware
bool
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
{
	/* select onboard/offboard mission */
	int *mission_index_ptr;
	dm_item_t dm_item;

	struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
	int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;

	/* do not work on empty missions */
	if (mission->count == 0) {
		return false;
	}

	/* move to next item if there is one */
	if (mission_index_next < ((int)mission->count - 1)) {
		mission_index_next++;
	}

	if (onboard) {
		/* onboard mission */
		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
		dm_item = DM_KEY_WAYPOINTS_ONBOARD;

	} else {
		/* offboard mission */
		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
	}

	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
	for (int i = 0; i < 10; i++) {

		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
			/* mission item index out of bounds - if they are equal, we just reached the end */
			if (*mission_index_ptr != (int)mission->count) {
				mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
			}
			return false;
		}

		const ssize_t len = sizeof(struct mission_item_s);

		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
		struct mission_item_s mission_item_tmp;

		/* read mission item from datamanager */
		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
			                     "ERROR waypoint could not be read");
			return false;
		}

		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
		if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {

			/* do DO_JUMP as many times as requested */
			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {

				/* only raise the repeat count if this is for the current mission item
				* but not for the next mission item */
				if (is_current) {
					(mission_item_tmp.do_jump_current_count)++;
					/* save repeat count */
					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
					    &mission_item_tmp, len) != len) {
						/* not supposed to happen unless the datamanager can't access the
						 * dataman */
						mavlink_log_critical(_navigator->get_mavlink_fd(),
								     "ERROR DO JUMP waypoint could not be written");
						return false;
					}
					report_do_jump_mission_changed(*mission_index_ptr,
								       mission_item_tmp.do_jump_repeat_count);
				}
				/* set new mission item index and repeat
				* we don't have to validate here, if it's invalid, we should realize this later .*/
				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;

			} else {
				if (is_current) {
					mavlink_log_critical(_navigator->get_mavlink_fd(),
							     "DO JUMP repetitions completed");
				}
				/* no more DO_JUMPS, therefore just try to continue with next mission item */
				(*mission_index_ptr)++;
			}

		} else {
			/* if it's not a DO_JUMP, then we were successful */
			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
			return true;
		}
	}

	/* we have given up, we don't want to cycle forever */
	mavlink_log_critical(_navigator->get_mavlink_fd(),
			     "ERROR DO JUMP is cycling, giving up");
	return false;
}
コード例 #26
0
enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position)
{
	const unsigned ndim = 3;
	
	struct sensor_combined_s sensor;
	float		accel_ema[ndim] = { 0.0f };		// exponential moving average of accel
	float		accel_disp[3] = { 0.0f, 0.0f, 0.0f };	// max-hold dispersion of accel
	float		ema_len = 0.5f;				// EMA time constant in seconds
	const float	normal_still_thr = 0.25;		// normal still threshold
	float		still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
	float		accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
	hrt_abstime	still_time = lenient_still_position ? 500000 : 1300000;	// still time required in us
    
	px4_pollfd_struct_t fds[1];
	fds[0].fd = accel_sub;
	fds[0].events = POLLIN;
	
	hrt_abstime t_start = hrt_absolute_time();
	/* set timeout to 30s */
	hrt_abstime timeout = 30000000;
	hrt_abstime t_timeout = t_start + timeout;
	hrt_abstime t = t_start;
	hrt_abstime t_prev = t_start;
	hrt_abstime t_still = 0;
	
	unsigned poll_errcount = 0;
	
	while (true) {
		/* wait blocking for new data */
		int poll_ret = px4_poll(fds, 1, 1000);
		
		if (poll_ret) {
			orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
			t = hrt_absolute_time();
			float dt = (t - t_prev) / 1000000.0f;
			t_prev = t;
			float w = dt / ema_len;
			
			for (unsigned i = 0; i < ndim; i++) {
				
				float di = 0.0f;
				switch (i) {
					case 0:
						di = sensor.accelerometer_m_s2[0];
						break;
					case 1:
						di = sensor.accelerometer_m_s2[1];
						break;
					case 2:
						di = sensor.accelerometer_m_s2[2];
						break;
				}
				
				float d = di - accel_ema[i];
				accel_ema[i] += d * w;
				d = d * d;
				accel_disp[i] = accel_disp[i] * (1.0f - w);
				
				if (d > still_thr2 * 8.0f) {
					d = still_thr2 * 8.0f;
				}
				
				if (d > accel_disp[i]) {
					accel_disp[i] = d;
				}
			}
			
			/* still detector with hysteresis */
			if (accel_disp[0] < still_thr2 &&
			    accel_disp[1] < still_thr2 &&
			    accel_disp[2] < still_thr2) {
				/* is still now */
				if (t_still == 0) {
					/* first time */
					mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still...");
					t_still = t;
					t_timeout = t + timeout;
					
				} else {
					/* still since t_still */
					if (t > t_still + still_time) {
						/* vehicle is still, exit from the loop to detection of its orientation */
						break;
					}
				}
				
			} else if (accel_disp[0] > still_thr2 * 4.0f ||
				   accel_disp[1] > still_thr2 * 4.0f ||
				   accel_disp[2] > still_thr2 * 4.0f) {
				/* not still, reset still start time */
				if (t_still != 0) {
					mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
					usleep(200000);
					t_still = 0;
				}
			}
			
		} else if (poll_ret == 0) {
			poll_errcount++;
		}
		
		if (t > t_timeout) {
			poll_errcount++;
		}
		
		if (poll_errcount > 1000) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
			return DETECT_ORIENTATION_ERROR;
		}
	}

	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return DETECT_ORIENTATION_TAIL_DOWN;        // [ g, 0, 0 ]
	}
	
	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return DETECT_ORIENTATION_NOSE_DOWN;        // [ -g, 0, 0 ]
	}
	
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return DETECT_ORIENTATION_LEFT;        // [ 0, g, 0 ]
	}
	
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return DETECT_ORIENTATION_RIGHT;        // [ 0, -g, 0 ]
	}
	
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
		return DETECT_ORIENTATION_UPSIDE_DOWN;        // [ 0, 0, g ]
	}
	
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
		return DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
	}
	
	mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation");
	
	return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
}
コード例 #27
0
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
	calibrate_return result = calibrate_return_ok;
	
	unsigned int calibration_counter_side;

	mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
	
	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);

	/*
	 * Detect if the system is rotating.
	 *
	 * We're detecting this as a general rotation on any axis, not necessary on the one we
	 * asked the user for. This is because we really just need two roughly orthogonal axes
	 * for a good result, so we're not constraining the user more than we have to.
	 */

	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
	hrt_abstime last_gyro = 0;
	float gyro_x_integral = 0.0f;
	float gyro_y_integral = 0.0f;
	float gyro_z_integral = 0.0f;

	const float gyro_int_thresh_rad = 0.5f;

	int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));

	while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
		fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
		fabsf(gyro_z_integral) < gyro_int_thresh_rad) {

		/* abort on request */
		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
			result = calibrate_return_cancelled;
			close(sub_gyro);
			return result;
		}

		/* abort with timeout */
		if (hrt_absolute_time() > detection_deadline) {
			result = calibrate_return_error;
			warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
			mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
			break;
		}

		/* Wait clocking for new data on all gyro */
		struct pollfd fds[1];
		fds[0].fd = sub_gyro;
		fds[0].events = POLLIN;
		size_t fd_count = 1;

		int poll_ret = poll(fds, fd_count, 1000);

		if (poll_ret > 0) {
			struct gyro_report gyro;
			orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);

			/* ensure we have a valid first timestamp */
			if (last_gyro > 0) {

				/* integrate */
				float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
				gyro_x_integral += gyro.x * delta_t;
				gyro_y_integral += gyro.y * delta_t;
				gyro_z_integral += gyro.z * delta_t;
			}

			last_gyro = gyro.timestamp;
		}
	}

	close(sub_gyro);
	
	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
	unsigned poll_errcount = 0;
	
	calibration_counter_side = 0;
	
	while (hrt_absolute_time() < calibration_deadline &&
	       calibration_counter_side < worker_data->calibration_points_perside) {
		
		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
			result = calibrate_return_cancelled;
			break;
		}
		
		// Wait clocking for new data on all mags
		px4_pollfd_struct_t fds[max_mags];
		size_t fd_count = 0;
		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (worker_data->sub_mag[cur_mag] >= 0) {
				fds[fd_count].fd = worker_data->sub_mag[cur_mag];
				fds[fd_count].events = POLLIN;
				fd_count++;
			}
		}
		int poll_ret = px4_poll(fds, fd_count, 1000);
		
		if (poll_ret > 0) {

			int prev_count[max_mags];
			bool rejected = false;

			for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {

				prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];

				if (worker_data->sub_mag[cur_mag] >= 0) {
					struct mag_report mag;

					orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);

					// Check if this measurement is good to go in
					rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
						worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
						worker_data->calibration_counter_total[cur_mag],
						calibration_sides * worker_data->calibration_points_perside);
					
					worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
					worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
					worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
					worker_data->calibration_counter_total[cur_mag]++;
				}
			}

			// Keep calibration of all mags in lockstep
			if (rejected) {
				// Reset counts, since one of the mags rejected the measurement
				for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
					worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
				}
			} else {
				calibration_counter_side++;

				// Progress indicator for side
				mavlink_and_console_log_info(worker_data->mavlink_fd,
							     "[cal] %s side calibration: progress <%u>",
							     detect_orientation_str(orientation),
							     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
			}
		} else {
			poll_errcount++;
		}
		
		if (poll_errcount > worker_data->calibration_points_perside * 3) {
			result = calibrate_return_error;
			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
			break;
		}
	}
	
	if (result == calibrate_return_ok) {
		mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
		
		worker_data->done_count++;
		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
	}
	
	return result;
}
コード例 #28
0
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
{
	/* 45 seconds */
	uint64_t calibration_interval = 25 * 1000 * 1000;

	/* maximum 500 values */
	const unsigned int calibration_maxcount = 240;
	unsigned int calibration_counter;

	float *x = NULL;
	float *y = NULL;
	float *z = NULL;

	char str[30];
	int res = OK;
	
	/* allocate memory */
	mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);

	x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
	y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
	z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));

	if (x == NULL || y == NULL || z == NULL) {
		mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");

		/* clean up */
		if (x != NULL) {
			free(x);
		}

		if (y != NULL) {
			free(y);
		}

		if (z != NULL) {
			free(z);
		}

		res = ERROR;
		return res;
	}

	if (res == OK) {
		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);

		if (sub_mag < 0) {
			mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort");
			res = ERROR;
		}  else {
			struct mag_report mag;

			/* limit update rate to get equally spaced measurements over time (in ms) */
			orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);

			/* calibrate offsets */
			uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
			unsigned poll_errcount = 0;

			mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");

			calibration_counter = 0U;

			while (hrt_absolute_time() < calibration_deadline &&
			       calibration_counter < calibration_maxcount) {

				/* wait blocking for new data */
				struct pollfd fds[1];
				fds[0].fd = sub_mag;
				fds[0].events = POLLIN;

				int poll_ret = poll(fds, 1, 1000);

				if (poll_ret > 0) {
					orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);

					x[calibration_counter] = mag.x;
					y[calibration_counter] = mag.y;
					z[calibration_counter] = mag.z;

					calibration_counter++;

					if (calibration_counter % (calibration_maxcount / 20) == 0) {
						mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
					}

				} else {
					poll_errcount++;
				}

				if (poll_errcount > 1000) {
					mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
					res = ERROR;
					break;
				}
			}

			close(sub_mag);
		}
	}

	float sphere_x;
	float sphere_y;
	float sphere_z;
	float sphere_radius;

	if (res == OK && calibration_counter > (calibration_maxcount / 2)) {

		/* sphere fit */
		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);

		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
			mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
			res = ERROR;
		}
	}

	if (x != NULL) {
		free(x);
	}

	if (y != NULL) {
		free(y);
	}

	if (z != NULL) {
		free(z);
	}

	if (res == OK) {
		/* apply calibration and set parameters */
		struct mag_scale mscale;
		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
		int fd = open(str, 0);
		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);

		if (res != OK) {
			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
		}

		if (res == OK) {
			mscale.x_offset = sphere_x;
			mscale.y_offset = sphere_y;
			mscale.z_offset = sphere_z;

			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);

			if (res != OK) {
				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
			}
		}

		close(fd);

		if (res == OK) {

			bool failed = false;
			/* set parameters */
			(void)sprintf(str, "CAL_MAG%u_ID", s);
			failed |= (OK != param_set(param_find(str), &(device_id)));
			(void)sprintf(str, "CAL_MAG%u_XOFF", s);
			failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
			(void)sprintf(str, "CAL_MAG%u_YOFF", s);
			failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
			(void)sprintf(str, "CAL_MAG%u_ZOFF", s);
			failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
			(void)sprintf(str, "CAL_MAG%u_XSCALE", s);
			failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
			(void)sprintf(str, "CAL_MAG%u_YSCALE", s);
			failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
			(void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
			failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));

			if (failed) {
				res = ERROR;
				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
			}

			mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
		}

		mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
				 (double)mscale.y_offset, (double)mscale.z_offset);
		mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
				 (double)mscale.y_scale, (double)mscale.z_scale);
	}

	return res;
}
コード例 #29
0
int do_mag_calibration(int mavlink_fd)
{
	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);

	struct mag_scale mscale_null = {
		0.0f,
		1.0f,
		0.0f,
		1.0f,
		0.0f,
		1.0f,
	};

	int result = OK;
	
	// Determine which mags are available and reset each

	int32_t	device_ids[max_mags];
	char str[30];

	for (size_t i=0; i<max_mags; i++) {
		device_ids[i] = 0; // signals no mag
	}
	
	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
		// Reset mag id to mag not available
		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
		if (result != OK) {
			mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
			break;
		}

		// Attempt to open mag
		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
		int fd = px4_open(str, O_RDONLY);
		if (fd < 0) {
			continue;
		}

		// Get device id for this mag
		device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);

		// Reset mag scale
		result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);

		if (result != OK) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
		}

		/* calibrate range */
		if (result == OK) {
			result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);

			if (result != OK) {
				mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
				/* this is non-fatal - mark it accordingly */
				result = OK;
			}
		}

		px4_close(fd);
	}

	// Calibrate all mags at the same time
	if (result == OK) {
		switch (mag_calibrate_all(mavlink_fd, device_ids)) {
			case calibrate_return_cancelled:
				// Cancel message already displayed, we're done here
				result = ERROR;
				break;
				
			case calibrate_return_ok:
				/* auto-save to EEPROM */
				result = param_save_default();

				/* if there is a any preflight-check system response, let the barrage of messages through */
				usleep(200000);

				if (result == OK) {
					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
					break;
				} else {
					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
				}
				// Fall through
				
			default:
				mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
				break;
		}
	}

	/* give this message enough time to propagate */
	usleep(600000);
	
	return result;
}
コード例 #30
0
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro,
		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
	bool failed = false;

	/* ---- MAG ---- */
	if (checkMag) {
		bool prime_found = false;
		int32_t prime_id = 0;
		param_get(param_find("CAL_MAG_PRIME"), &prime_id);

		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_mag_count; i++) {
			bool required = (i < max_mandatory_mag_count);
			int device_id = -1;

			if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
				failed = true;
			}

			if (device_id == prime_id) {
				prime_found = true;
			}
		}

		/* check if the primary device is present */
		if (!prime_found && prime_id != 0) {
			if (reportFailures) {
				mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary compass not found");
			}
			failed = true;
		}
	}

	/* ---- ACCEL ---- */
	if (checkAcc) {
		bool prime_found = false;
		int32_t prime_id = 0;
		param_get(param_find("CAL_ACC_PRIME"), &prime_id);

		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_accel_count; i++) {
			bool required = (i < max_mandatory_accel_count);
			int device_id = -1;

			if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, reportFailures) && required) {
				failed = true;
			}

			if (device_id == prime_id) {
				prime_found = true;
			}
		}

		/* check if the primary device is present */
		if (!prime_found && prime_id != 0) {
			if (reportFailures) {
				mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary accelerometer not found");
			}
			failed = true;
		}
	}

	/* ---- GYRO ---- */
	if (checkGyro) {
		bool prime_found = false;
		int32_t prime_id = 0;
		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);

		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
			bool required = (i < max_mandatory_gyro_count);
			int device_id = -1;

			if (!gyroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
				failed = true;
			}

			if (device_id == prime_id) {
				prime_found = true;
			}
		}

		/* check if the primary device is present */
		if (!prime_found && prime_id != 0) {
			if (reportFailures) {
				mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary gyro not found");
			}
			failed = true;
		}
	}

	/* ---- BARO ---- */
	if (checkBaro) {
		bool prime_found = false;
		int32_t prime_id = 0;
		param_get(param_find("CAL_BARO_PRIME"), &prime_id);

		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_baro_count; i++) {
			bool required = (i < max_mandatory_baro_count);
			int device_id = -1;

			if (!baroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
				failed = true;
			}

			if (device_id == prime_id) {
				prime_found = true;
			}
		}

		// TODO there is no logic in place to calibrate the primary baro yet
		// // check if the primary device is present
		if (!prime_found && prime_id != 0) {
			if (reportFailures) {
				mavlink_and_console_log_critical(mavlink_log_pub, "warning: primary barometer not operational");
			}
			failed = true;
		}
	}

	/* ---- AIRSPEED ---- */
	if (checkAirspeed) {
		if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) {
			failed = true;
		}
	}

	/* ---- RC CALIBRATION ---- */
	if (checkRC) {
		if (rc_calibration_check(mavlink_log_pub, reportFailures) != OK) {
			if (reportFailures) {
				mavlink_and_console_log_critical(mavlink_log_pub, "RC calibration check failed");
			}
			failed = true;
		}
	}

	/* ---- Global Navigation Satellite System receiver ---- */
	if (checkGNSS) {
		if (!gnssCheck(mavlink_log_pub, reportFailures)) {
			failed = true;
		}
	}


#ifdef __PX4_QURT
	// WARNING: Preflight checks are important and should be added back when
	// all the sensors are supported
	PX4_WARN("WARNING: Preflight checks PASS always.");
	failed = false;
#endif

	/* Report status */
	return !failed;
}