Пример #1
0
void
Navigator::task_main()
{
	bool have_geofence_position_data = false;

	/* 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) {
		PX4_INFO("Try to load geofence.txt");
		_geofence.loadFromFile(GEOFENCE_FILENAME);

	} else {
		if (_geofence.clearDm() != OK) {
			mavlink_log_critical(&_mavlink_log_pub, "failed clearing 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));
	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_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));
	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

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

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[1] = {};

	/* Setup of loop */
	fds[0].fd = _global_pos_sub;
	fds[0].events = POLLIN;

	bool global_pos_available_once = false;

	while (!_task_should_exit) {

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

		if (pret == 0) {
			/* timed out - periodic check for _task_should_exit, etc. */
			if (global_pos_available_once) {
				global_pos_available_once = false;
				PX4_WARN("global position timeout");
			}
			/* Let the loop run anyway, don't do `continue` here. */

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

			if (fds[0].revents & POLLIN) {
				/* success, global pos is available */
				global_position_update();
				if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
					have_geofence_position_data = true;
				}
				global_pos_available_once = true;
			}
		}

		perf_begin(_loop_perf);

		bool updated;

		/* gps updated */
		orb_check(_gps_pos_sub, &updated);
		if (updated) {
			gps_position_update();
			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
				have_geofence_position_data = true;
			}
		}

		/* sensors combined updated */
		orb_check(_sensor_combined_sub, &updated);
		if (updated) {
			sensor_combined_update();
		}

		/* parameters updated */
		orb_check(_param_update_sub, &updated);
		if (updated) {
			params_update();
			updateParams();
		}

		/* vehicle control mode updated */
		orb_check(_control_mode_sub, &updated);
		if (updated) {
			vehicle_control_mode_update();
		}

		/* vehicle status updated */
		orb_check(_vstatus_sub, &updated);
		if (updated) {
			vehicle_status_update();
		}

		/* vehicle land detected updated */
		orb_check(_land_detected_sub, &updated);
		if (updated) {
			vehicle_land_detected_update();
		}

		/* navigation capabilities updated */
		orb_check(_fw_pos_ctrl_status_sub, &updated);
		if (updated) {
			fw_pos_ctrl_status_update();
		}

		/* home position updated */
		orb_check(_home_pos_sub, &updated);
		if (updated) {
			home_position_update();
		}

		orb_check(_vehicle_command_sub, &updated);
		if (updated) {
			vehicle_command_s cmd;
			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {

				struct position_setpoint_triplet_s *rep = get_reposition_triplet();

				// store current position as previous position and goal as next
				rep->previous.yaw = get_global_position()->yaw;
				rep->previous.lat = get_global_position()->lat;
				rep->previous.lon = get_global_position()->lon;
				rep->previous.alt = get_global_position()->alt;

				rep->current.loiter_radius = get_loiter_radius();
				rep->current.loiter_direction = 1;
				rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;

				// Go on and check which changes had been requested
				if (PX4_ISFINITE(cmd.param4)) {
					rep->current.yaw = cmd.param4;
				} else {
					rep->current.yaw = NAN;
				}

				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;

				} else {
					rep->current.lat = get_global_position()->lat;
					rep->current.lon = get_global_position()->lon;
				}

				if (PX4_ISFINITE(cmd.param7)) {
					rep->current.alt = cmd.param7;
				} else {
					rep->current.alt = get_global_position()->alt;
				}

				rep->previous.valid = true;
				rep->current.valid = true;
				rep->next.valid = false;
			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
				struct position_setpoint_triplet_s *rep = get_takeoff_triplet();

				// store current position as previous position and goal as next
				rep->previous.yaw = get_global_position()->yaw;
				rep->previous.lat = get_global_position()->lat;
				rep->previous.lon = get_global_position()->lon;
				rep->previous.alt = get_global_position()->alt;

				rep->current.loiter_radius = get_loiter_radius();
				rep->current.loiter_direction = 1;
				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
				rep->current.yaw = cmd.param4;

				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
				} else {
					// If one of them is non-finite, reset both
					rep->current.lat = NAN;
					rep->current.lon = NAN;
				}

				rep->current.alt = cmd.param7;

				rep->previous.valid = true;
				rep->current.valid = true;
				rep->next.valid = false;

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {

				/* find NAV_CMD_DO_LAND_START in the mission and
				 * use MAV_CMD_MISSION_START to start the mission there
				 */
				unsigned land_start = _mission.find_offboard_land_start();
				if (land_start != -1) {
					vehicle_command_s vcmd = {};
					vcmd.target_system = get_vstatus()->system_id;
					vcmd.target_component = get_vstatus()->component_id;
					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
					vcmd.param1 = land_start;
					vcmd.param2 = 0;

					orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
					(void)orb_unadvertise(pub);
				}

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {

				if (get_mission_result()->valid &&
					PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {

					_mission.set_current_offboard_mission_index(cmd.param1);
				}

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
				PX4_INFO("got pause/continue command");
			}
		}

		/* Check geofence violation */
		static hrt_abstime last_geofence_check = 0;
		if (have_geofence_position_data &&
			(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
			(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {

			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
			last_geofence_check = hrt_absolute_time();
			have_geofence_position_data = false;

			_geofence_result.geofence_action = _geofence.getGeofenceAction();
			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_log_pub, "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 vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_mission;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_loiter;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
				_pos_sp_triplet_published_invalid_once = false;
				if (_param_rcloss_act.get() == 1) {
					_navigation_mode = &_loiter;
				} else if (_param_rcloss_act.get() == 3) {
					_navigation_mode = &_land;
				} else if (_param_rcloss_act.get() == 4) {
					_navigation_mode = &_rcLoss;
				} else { /* if == 2 or unknown, RTL */
					_navigation_mode = &_rtl;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_rtl;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_takeoff;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_land;
				break;
			case vehicle_status_s::NAVIGATION_STATE_DESCEND:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_land;
				break;
			case vehicle_status_s::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_act.get() == 1) {
					_navigation_mode = &_loiter;
				} else if (_param_datalinkloss_act.get() == 3) {
					_navigation_mode = &_land;
				} else if (_param_datalinkloss_act.get() == 4) {
					_navigation_mode = &_dataLinkLoss;
				} else { /* if == 2 or unknown, RTL */
					_navigation_mode = &_rtl;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_engineFailure;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_gpsFailure;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_follow_target;
				break;
			case vehicle_status_s::NAVIGATION_STATE_MANUAL:
			case vehicle_status_s::NAVIGATION_STATE_ACRO:
			case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
			case vehicle_status_s::NAVIGATION_STATE_POSCTL:
			case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
			case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
			case vehicle_status_s::NAVIGATION_STATE_STAB:
			default:
				_pos_sp_triplet_published_invalid_once = false;
				_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) {
			_pos_sp_triplet.timestamp = hrt_absolute_time();
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

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

		perf_end(_loop_perf);
	}
	PX4_INFO("exiting");

	_navigator_task = -1;
	return;
}
Пример #2
0
void
controls_tick()
{

	/*
	 * Gather R/C control inputs from supported sources.
	 *
	 * Note that if you're silly enough to connect more than
	 * one control input source, they're going to fight each
	 * other.  Don't do that.
	 */

	/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
	uint16_t rssi = 0;

#ifdef ADC_RSSI

	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
		unsigned counts = adc_measure(ADC_RSSI);

		if (counts != 0xffff) {
			/* low pass*/
			_rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f);
			/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
			unsigned mV = _rssi_adc_counts * 3300 / 4095;
			/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
			rssi = (mV * RC_INPUT_RSSI_MAX / 3300);

			if (rssi > RC_INPUT_RSSI_MAX) {
				rssi = RC_INPUT_RSSI_MAX;
			}
		}
	}

#endif

	/* zero RSSI if signal is lost */
	if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) {
		rssi = 0;
	}

	perf_begin(c_gather_dsm);
	bool dsm_updated, st24_updated, sumd_updated;
	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);

	if (dsm_updated) {
		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
	}

	if (st24_updated) {
		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
	}

	if (sumd_updated) {
		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
	}

	perf_end(c_gather_dsm);

	perf_begin(c_gather_sbus);

	bool sbus_failsafe, sbus_frame_drop;
	bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
				       PX4IO_RC_INPUT_CHANNELS);

	if (sbus_updated) {
		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;

		unsigned sbus_rssi = RC_INPUT_RSSI_MAX;

		if (sbus_frame_drop) {
			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
			sbus_rssi = RC_INPUT_RSSI_MAX / 2;

		} else {
			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		}

		if (sbus_failsafe) {
			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;

		} else {
			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
		}

		/* set RSSI to an emulated value if ADC RSSI is off */
		if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
			rssi = sbus_rssi;
		}

	}

	perf_end(c_gather_sbus);

	/*
	 * XXX each S.bus frame will cause a PPM decoder interrupt
	 * storm (lots of edges).  It might be sensible to actually
	 * disable the PPM decoder completely if we have S.bus signal.
	 */
	perf_begin(c_gather_ppm);
	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);

	if (ppm_updated) {

		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
	}

	perf_end(c_gather_ppm);

	/* limit number of channels to allowable data size */
	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) {
		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
	}

	/* store RSSI */
	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;

	/*
	 * In some cases we may have received a frame, but input has still
	 * been lost.
	 */
	bool rc_input_lost = false;

	/*
	 * If we received a new frame from any of the RC sources, process it.
	 */
	if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {

		/* record a bitmask of channels assigned */
		unsigned assigned_channels = 0;

		/* update RC-received timestamp */
		system_state.rc_channels_timestamp_received = hrt_absolute_time();

		/* update RC-received timestamp */
		system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;

		/* map raw inputs to mapped inputs */
		/* XXX mapping should be atomic relative to protocol */
		for (unsigned i = 0; i < r_raw_rc_count; i++) {

			/* map the input channel */
			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];

			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {

				uint16_t raw = r_raw_rc_values[i];

				int16_t scaled;

				/*
				 * 1) Constrain to min/max values, as later processing depends on bounds.
				 */
				if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
					raw = conf[PX4IO_P_RC_CONFIG_MIN];
				}

				if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
					raw = conf[PX4IO_P_RC_CONFIG_MAX];
				}

				/*
				 * 2) Scale around the mid point differently for lower and upper range.
				 *
				 * This is necessary as they don't share the same endpoints and slope.
				 *
				 * First normalize to 0..1 range with correct sign (below or above center),
				 * then scale to 20000 range (if center is an actual center, -10000..10000,
				 * if parameters only support half range, scale to 10000 range, e.g. if
				 * center == min 0..10000, if center == max -10000..0).
				 *
				 * As the min and max bounds were enforced in step 1), division by zero
				 * cannot occur, as for the case of center == min or center == max the if
				 * statement is mutually exclusive with the arithmetic NaN case.
				 *
				 * DO NOT REMOVE OR ALTER STEP 1!
				 */
				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
								     conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));

				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
								     conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));

				} else {
					/* in the configured dead zone, output zero */
					scaled = 0;
				}

				/* invert channel if requested */
				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
					scaled = -scaled;
				}

				/* and update the scaled/mapped version */
				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];

				if (mapped < PX4IO_CONTROL_CHANNELS) {

					/* invert channel if pitch - pulling the lever down means pitching up by convention */
					if (mapped == 1) {
						/* roll, pitch, yaw, throttle, override is the standard order */
						scaled = -scaled;
					}

					if (mapped == 3 && r_setup_rc_thr_failsafe) {
						/* throttle failsafe detection */
						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;

						} else {
							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
						}
					}

					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
					assigned_channels |= (1 << mapped);

				} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
					/* pick out override channel, indicated by special mapping */
					rc_value_override = SIGNED_TO_REG(scaled);
				}
			}
		}

		/* set un-assigned controls to zero */
		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
			if (!(assigned_channels & (1 << i))) {
				r_rc_values[i] = 0;
			}
		}

		/* set RC OK flag, as we got an update */
		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
		r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;

		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
		if (assigned_channels > 4) {
			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;

		} else {
			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
		}

		/*
		 * Export the valid channel bitmap
		 */
		r_rc_valid = assigned_channels;
	}

	/*
	 * If we haven't seen any new control data in 200ms, assume we
	 * have lost input.
	 */
	if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
		rc_input_lost = true;

		/* clear the input-kind flags here */
		r_status_flags &= ~(
					  PX4IO_P_STATUS_FLAGS_RC_PPM |
					  PX4IO_P_STATUS_FLAGS_RC_DSM |
					  PX4IO_P_STATUS_FLAGS_RC_SBUS);

	}

	/*
	 * Handle losing RC input
	 */

	/* if we are in failsafe, clear the override flag */
	if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
	}

	/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
	if (rc_input_lost) {
		/* Clear the RC input status flag, clear manual override flag */
		r_status_flags &= ~(
					  PX4IO_P_STATUS_FLAGS_OVERRIDE |
					  PX4IO_P_STATUS_FLAGS_RC_OK);

		/* flag raw RC as lost */
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);

		/* Mark all channels as invalid, as we just lost the RX */
		r_rc_valid = 0;

		/* Set raw channel count to zero */
		r_raw_rc_count = 0;

		/* Set the RC_LOST alarm */
		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
	}

	/*
	 * Check for manual override.
	 *
	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
	 * must have R/C input (NO FAILSAFE!).
	 * Override is enabled if either the hardcoded channel / value combination
	 * is selected, or the AP has requested it.
	 */
	if ((!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)) &&
	    (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
	    !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {

		bool override = false;

		/*
		 * Check mapped channel 5 (can be any remote channel,
		 * depends on RC_MAP_OVER parameter);
		 * If the value is 'high' then the pilot has
		 * requested override.
		 *
		 */
		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
			override = true;
		}
Пример #3
0
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
{
	perf_begin(c_gather_dsm);
	uint16_t temp_count = r_raw_rc_count;
	uint8_t n_bytes = 0;
	uint8_t *bytes;
	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS);

	if (*dsm_updated) {
		r_raw_rc_count = temp_count & 0x7fff;

		if (temp_count & 0x8000) {
			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;

		} else {
			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
		}

		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);

	}

	perf_end(c_gather_dsm);

	/* get data from FD and attempt to parse with DSM and ST24 libs */
	uint8_t st24_rssi, rx_count;
	uint16_t st24_channel_count = 0;

	*st24_updated = false;

	for (unsigned i = 0; i < n_bytes; i++) {
		/* set updated flag if one complete packet was parsed */
		st24_rssi = RC_INPUT_RSSI_MAX;
		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
						    &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
	}

	if (*st24_updated) {

		/* ensure ADC RSSI is disabled */
		r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);

		*rssi = st24_rssi;
		r_raw_rc_count = st24_channel_count;

		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
	}


	/* get data from FD and attempt to parse with SUMD libs */
	uint8_t sumd_rssi, sumd_rx_count;
	uint16_t sumd_channel_count = 0;

	*sumd_updated = false;

	for (unsigned i = 0; i < n_bytes; i++) {
		/* set updated flag if one complete packet was parsed */
		sumd_rssi = RC_INPUT_RSSI_MAX;
		*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
						    &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
	}

	if (*sumd_updated) {

		/* not setting RSSI since SUMD does not provide one */
		r_raw_rc_count = sumd_channel_count;

		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
	}

	return (*dsm_updated | *st24_updated | *sumd_updated);
}
Пример #4
0
void
BMI055_gyro::measure()
{
	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct BMI_GyroReport bmi_gyroreport;

	struct Report {
		int16_t     temp;
		int16_t     gyro_x;
		int16_t     gyro_y;
		int16_t     gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the BMI055 gyro in one pass.
	 */
	bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ;


	if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
		return;
	}

	check_registers();

	uint8_t temp = read_reg(BMI055_ACC_TEMP);

	report.temp = temp;

	report.gyro_x = bmi_gyroreport.gyro_x;
	report.gyro_y = bmi_gyroreport.gyro_y;
	report.gyro_z = bmi_gyroreport.gyro_z;

	if (report.temp == 0 &&
	    report.gyro_x == 0 &&
	    report.gyro_y == 0 &&
	    report.gyro_z == 0) {
		// all zero data - probably a SPI bus error
		perf_count(_bad_transfers);
		perf_end(_sample_perf);
		// note that we don't call reset() here as a reset()
		// costs 20ms with interrupts disabled. That means if
		// the bmi055 does go bad it would cause a FMU failure,
		// regardless of whether another sensor is available,
		return;
	}

	perf_count(_good_transfers);

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again. We still increment
		// _good_transfers, but don't return any data yet
		_register_wait--;
		return;
	}

	/*
	 * Report buffers.
	 */
	gyro_report     grb;


	grb.timestamp =  hrt_absolute_time();

	// report the error count as the sum of the number of bad
	// transfers and bad register reads. This allows the higher
	// level code to decide if it should use this sensor based on
	// whether it has had failures
	grb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 *   at a nominally 'zero' input. Therefore the offset has to
	 *   be subtracted.
	 *
	 *   Example: A gyro outputs a value of 74 at zero angular rate
	 *        the offset is 74 from the origin and subtracting
	 *        74 from all measurements centers them around zero.
	 */

	grb.x_raw = report.gyro_x;
	grb.y_raw = report.gyro_y;
	grb.z_raw = report.gyro_z;

	float xraw_f = report.gyro_x;
	float yraw_f = report.gyro_y;
	float zraw_f = report.gyro_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
	grb.z = _gyro_filter_z.apply(z_gyro_in_new);

	matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	matrix::Vector3f gval_integrated;

	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt);
	grb.x_integral = gval_integrated(0);
	grb.y_integral = gval_integrated(1);
	grb.z_integral = gval_integrated(2);

	grb.scaling = _gyro_range_scale;
	grb.range_rad_s = _gyro_range_rad_s;

	grb.temperature_raw = report.temp;
	grb.temperature = _last_temperature;
	grb.device_id = _device_id.devid;

	_gyro_reports->force(&grb);

	/* notify anyone waiting for data */
	if (gyro_notify) {
		poll_notify(POLLIN);
	}

	if (gyro_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
Пример #5
0
int
SF0X::collect()
{
	int	ret;

	perf_begin(_sample_perf);

	/* clear buffer if last read was too long ago */
	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

	/* the buffer for read chars is buflen minus null termination */
	char readbuf[sizeof(_linebuf)];
	unsigned readlen = sizeof(readbuf) - 1;

	/* read from the sensor (uart buffer) */
	ret = ::read(_fd, &readbuf[0], readlen);

	if (ret < 0) {
		DEVICE_DEBUG("read err: %d", ret);
		perf_count(_comms_errors);
		perf_end(_sample_perf);

		/* only throw an error if we time out */
		if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
			return ret;

		} else {
			return -EAGAIN;
		}
	} else if (ret == 0) {
		return -EAGAIN;
	}

	_last_read = hrt_absolute_time();

	float distance_m = -1.0f;
	bool valid = false;

	for (int i = 0; i < ret; i++) {
		if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
			valid = true;
		}
	}

	if (!valid) {
		return -EAGAIN;
	}

	DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));

	struct distance_sensor_s report;

	report.timestamp = hrt_absolute_time();
	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	report.orientation = 8;
	report.current_distance = distance_m;
	report.min_distance = get_minimum_distance();
	report.max_distance = get_maximum_distance();
	report.covariance = 0.0f;
	/* TODO: set proper ID */
	report.id = 0;

	/* publish it */
	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
Пример #6
0
int
SF0X::collect()
{
	int	ret;

	perf_begin(_sample_perf);

	/* clear buffer if last read was too long ago */
	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

	/* the buffer for read chars is buflen minus null termination */
	char readbuf[sizeof(_linebuf)];
	unsigned readlen = sizeof(readbuf) - 1;

	/* read from the sensor (uart buffer) */
	ret = ::read(_fd, &readbuf[0], readlen);

	if (ret < 0) {
		debug("read err: %d", ret);
		perf_count(_comms_errors);
		perf_end(_sample_perf);

		/* only throw an error if we time out */
		if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
			return ret;

		} else {
			return -EAGAIN;
		}
	} else if (ret == 0) {
		return -EAGAIN;
	}

	_last_read = hrt_absolute_time();

	float si_units;
	bool valid = false;

	for (int i = 0; i < ret; i++) {
		if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
			valid = true;
		}
	}

	if (!valid) {
		return -EAGAIN;
	}

	debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));

	struct range_finder_report report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.distance = si_units;
	report.minimum_distance = get_minimum_distance();
	report.maximum_distance = get_maximum_distance();
	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);

	/* publish it */
	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
Пример #7
0
int
LIS3MDL::collect()
{
#pragma pack(push, 1)
	struct {
		uint8_t x[2];
		uint8_t y[2];
		uint8_t z[2];
	}       lis_report;

	struct {
		int16_t x;
		int16_t y;
		int16_t z;
		int16_t t;
	} report;
#pragma pack(pop)

	int     ret = 0;
	uint8_t buf_rx[2] = {0};

	float xraw_f;
	float yraw_f;
	float zraw_f;

	struct mag_report new_mag_report;
	bool sensor_is_onboard = false;

	perf_begin(_sample_perf);

	new_mag_report.timestamp = hrt_absolute_time();
	new_mag_report.error_count = perf_event_count(_comms_errors);
	new_mag_report.scaling = _range_scale;
	new_mag_report.device_id = _device_id.devid;

	ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));

	/**
	 * Weird behavior: the X axis will be read instead of the temperature registers if you use a pointer to a packed struct...not sure why.
	 * This works now, but further investigation to determine why this happens would be good (I am guessing a type error somewhere)
	 */
	ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx));

	if (ret != OK) {
		perf_count(_comms_errors);
		PX4_WARN("Register read error.");
		return ret;
	}

	report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]);
	report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]);
	report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]);

	report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]);

	float temperature = report.t;
	new_mag_report.temperature = 25.0f + (temperature / 8.0f);

	// XXX revisit for SPI part, might require a bus type IOCTL

	unsigned dummy = 0;
	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
	new_mag_report.is_external = !sensor_is_onboard;

	/**
	 * RAW outputs
	 */
	new_mag_report.x_raw = report.x;
	new_mag_report.y_raw = report.y;
	new_mag_report.z_raw = report.z;

	xraw_f = report.x;
	yraw_f = report.y;
	zraw_f = report.z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
	/* flip axes and negate value for y */
	new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
	/* z remains z */
	new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;

	if (!(_pub_blocked)) {

		if (_mag_topic != nullptr) {
			/* publish it */
			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report);

		} else {
			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report,
							 &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);

			if (_mag_topic == nullptr) {
				DEVICE_DEBUG("ADVERT FAIL");
			}
		}
	}

	_last_report = new_mag_report;

	/* post a report to the ring */
	_reports->force(&new_mag_report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
Пример #8
0
void
FXAS21002C::measure()
{
	/* status register and data as read back from the device */

#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
	} raw_gyro_report;
#pragma pack(pop)

	struct gyro_report gyro_report;

	/* start the performance counter */
	perf_begin(_sample_perf);

	check_registers();

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again.
		_register_wait--;
		perf_end(_sample_perf);
		return;
	}

	/* fetch data from the sensor */
	memset(&raw_gyro_report, 0, sizeof(raw_gyro_report));
	raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS);
	transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report));

	if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) {
		perf_end(_sample_perf);
		perf_count(_duplicates);
		return;
	}

	/*
	 * The TEMP register contains an 8-bit 2's complement temperature value with a range
	 * of –128 °C to +127 °C and a scaling of 1 °C/LSB. The temperature data is only
	 * compensated (factory trim values applied) when the device is operating in the Active
	 * mode and actively measuring the angular rate.
	 */

	if ((_read % _current_rate) == 0) {
		_last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f;
		gyro_report.temperature = _last_temperature;
	}

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	gyro_report.timestamp = hrt_absolute_time();

	// report the error count as the number of bad
	// register reads. This allows the higher level
	// code to decide if it should use this sensor based on
	// whether it has had failures
	gyro_report.error_count = perf_event_count(_bad_registers);

	gyro_report.x_raw = swap16(raw_gyro_report.x);
	gyro_report.y_raw = swap16(raw_gyro_report.y);
	gyro_report.z_raw = swap16(raw_gyro_report.z);

	float xraw_f = gyro_report.x_raw;
	float yraw_f = gyro_report.y_raw;
	float zraw_f = gyro_report.z_raw;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	gyro_report.x = _gyro_filter_x.apply(x_in_new);
	gyro_report.y = _gyro_filter_y.apply(y_in_new);
	gyro_report.z = _gyro_filter_z.apply(z_in_new);

	matrix::Vector3f gval(x_in_new, y_in_new, z_in_new);
	matrix::Vector3f gval_integrated;

	bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
	gyro_report.x_integral = gval_integrated(0);
	gyro_report.y_integral = gval_integrated(1);
	gyro_report.z_integral = gval_integrated(2);

	gyro_report.scaling = _gyro_range_scale;
	gyro_report.range_rad_s = _gyro_range_rad_s;

	/* return device ID */
	gyro_report.device_id = _device_id.devid;


	_reports->force(&gyro_report);

	/* notify anyone waiting for data */
	if (gyro_notify) {
		poll_notify(POLLIN);

		if (!(_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
		}
	}

	_read++;

	/* stop the perf counter */
	perf_end(_sample_perf);
}
Пример #9
0
int
user_start(int argc, char *argv[])
{
	/* run C++ ctors before we go any further */
	up_cxxinitialize();

	/* reset all to zero */
	memset(&system_state, 0, sizeof(system_state));

	/* configure the high-resolution time/callout interface */
	hrt_init();

	/* calculate our fw CRC so FMU can decide if we need to update */
	calculate_fw_crc();

	/*
	 * Poll at 1ms intervals for received bytes that have not triggered
	 * a DMA event.
	 */
#ifdef CONFIG_ARCH_DMA
	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif

	/* print some startup info */
	lowsyslog("\nPX4IO: starting\n");

	/* default all the LEDs to off while we start */
	LED_AMBER(false);
	LED_BLUE(false);
	LED_SAFETY(false);
#ifdef GPIO_LED4
	LED_RING(false);
#endif

	/* turn on servo power (if supported) */
#ifdef POWER_SERVO
	POWER_SERVO(true);
#endif

	/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
	ENABLE_SBUS_OUT(false);
#endif

	/* start the safety switch handler */
	safety_init();

	/* configure the first 8 PWM outputs (i.e. all of them) */
	up_pwm_servo_init(0xff);

	/* initialise the control inputs */
	controls_init();

	/* set up the ADC */
	adc_init();

	/* start the FMU interface */
	interface_init();
#ifdef CONFIG_ARCH_BOARD_RASPILOTIO_V1
    spiuart_init(57600U);
#endif

	/* add a performance counter for mixing */
	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");

	/* add a performance counter for controls */
	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");

	/* and one for measuring the loop rate */
	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");

	struct mallinfo minfo = mallinfo();
	lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);

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

	/*
	 *    P O L I C E    L I G H T S
	 *
	 * Not enough memory, lock down.
	 *
	 * We might need to allocate mixers later, and this will
	 * ensure that a developer doing a change will notice
	 * that he just burned the remaining RAM with static
	 * allocations. We don't want him to be able to
	 * get past that point. This needs to be clearly
	 * documented in the dev guide.
         *
	 */
	if (minfo.mxordblk < 600) {

		lowsyslog("ERR: not enough MEM");
		bool phase = false;

		while (true) {

			if (phase) {
				LED_AMBER(true);
				LED_BLUE(false);
			} else {
				LED_AMBER(false);
				LED_BLUE(true);
			}
			up_udelay(250000);

			phase = !phase;
		}
	}

	/* Start the failsafe led init */
	failsafe_led_init();

	/*
	 * Run everything in a tight loop.
	 */

	uint64_t last_debug_time = 0;
        uint64_t last_heartbeat_time = 0;
	for (;;) {

		/* track the rate at which the loop is running */
		perf_count(loop_perf);

		/* kick the mixer */
		perf_begin(mixer_perf);
		mixer_tick();
		perf_end(mixer_perf);

		/* kick the control inputs */
		perf_begin(controls_perf);
		controls_tick();
		perf_end(controls_perf);

                if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
                    last_heartbeat_time = hrt_absolute_time();
                    heartbeat_blink();
                }

		ring_blink();

                check_reboot();

		/* check for debug activity (default: none) */
		show_debug_messages();

		/* post debug state at ~1Hz - this is via an auxiliary serial port
		 * DEFAULTS TO OFF!
		 */
		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {

			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", 
				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
				  (unsigned)r_status_flags,
				  (unsigned)r_setup_arming,
				  (unsigned)r_setup_features,
				  (unsigned)mallinfo().mxordblk);
			last_debug_time = hrt_absolute_time();
		}
	}
}
Пример #10
0
void
MPU9250::measure()
{
	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct MPUReport mpu_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the MPU9250 in one pass.
	 */
	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;

	// sensor transfer at high clock speed
	set_frequency(MPU9250_HIGH_BUS_SPEED);

	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) {
		return;
	}

	check_registers();

	if (check_duplicate(&mpu_report.accel_x[0])) {
		return;
	}

	_mag->measure(mpu_report.mag);

	/*
	 * Convert from big to little endian
	 */
	report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
	report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
	report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
	report.temp    = int16_t_from_bytes(mpu_report.temp);
	report.gyro_x  = int16_t_from_bytes(mpu_report.gyro_x);
	report.gyro_y  = int16_t_from_bytes(mpu_report.gyro_y);
	report.gyro_z  = int16_t_from_bytes(mpu_report.gyro_z);

	if (check_null_data((uint32_t *)&report, sizeof(report) / 4)) {
		return;
	}

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using the sensor again
		// We still increment _good_transfers, but don't return any data yet
		_register_wait--;
		return;
	}

	/*
	 * Swap axes and negate y
	 */
	int16_t accel_xt = report.accel_y;
	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);

	int16_t gyro_xt = report.gyro_y;
	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);

	/*
	 * Apply the swap
	 */
	report.accel_x = accel_xt;
	report.accel_y = accel_yt;
	report.gyro_x = gyro_xt;
	report.gyro_y = gyro_yt;

	#if defined(CONFIG_ARCH_BOARD_LUCI_V1)
		int16_t gyt = -report.gyro_y;
		int16_t gzt = -report.gyro_z;
		int16_t ayt = -report.accel_y;
		int16_t azt = -report.accel_z;
		report.gyro_z = gzt;
		report.gyro_y = gyt;
		report.accel_z = azt;
		report.accel_y = ayt;
	#endif

	/*
	 * Report buffers.
	 */
	accel_report		arb;
	gyro_report		grb;

	/*
	 * Adjust and scale results to m/s^2.
	 */
	grb.timestamp = arb.timestamp = hrt_absolute_time();

	// report the error count as the sum of the number of bad
	// transfers and bad register reads. This allows the higher
	// level code to decide if it should use this sensor based on
	// whether it has had failures
	grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	/* NOTE: Axes have been swapped to match the board a few lines above. */

	arb.x_raw = report.accel_x;
	arb.y_raw = report.accel_y;
	arb.z_raw = report.accel_z;

	float xraw_f = report.accel_x;
	float yraw_f = report.accel_y;
	float zraw_f = report.accel_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

	arb.x = _accel_filter_x.apply(x_in_new);
	arb.y = _accel_filter_y.apply(y_in_new);
	arb.z = _accel_filter_z.apply(z_in_new);

	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
	math::Vector<3> aval_integrated;

	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
	arb.x_integral = aval_integrated(0);
	arb.y_integral = aval_integrated(1);
	arb.z_integral = aval_integrated(2);

	arb.scaling = _accel_range_scale;
	arb.range_m_s2 = _accel_range_m_s2;

	_last_temperature = (report.temp) / 361.0f + 35.0f;

	arb.temperature_raw = report.temp;
	arb.temperature = _last_temperature;

	grb.x_raw = report.gyro_x;
	grb.y_raw = report.gyro_y;
	grb.z_raw = report.gyro_z;

	xraw_f = report.gyro_x;
	yraw_f = report.gyro_y;
	zraw_f = report.gyro_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
	grb.z = _gyro_filter_z.apply(z_gyro_in_new);

	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
	grb.x_integral = gval_integrated(0);
	grb.y_integral = gval_integrated(1);
	grb.z_integral = gval_integrated(2);

	grb.scaling = _gyro_range_scale;
	grb.range_rad_s = _gyro_range_rad_s;

	grb.temperature_raw = report.temp;
	grb.temperature = _last_temperature;

	_accel_reports->force(&arb);
	_gyro_reports->force(&grb);

	/* notify anyone waiting for data */
	if (accel_notify) {
		poll_notify(POLLIN);
	}

	if (gyro_notify) {
		_gyro->parent_poll_notify();
	}

	if (accel_notify && !(_pub_blocked)) {
		/* log the time of this report */
		perf_begin(_controller_latency_perf);
		/* publish it */
		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
	}

	if (gyro_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
Пример #11
0
void
MPU9250::measure()
{

	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct MPUReport mpu_report;

	struct ICMReport icm_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the MPU9250 in one pass
	 */

	if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) {
		if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {
			if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
				perf_end(_sample_perf);
				return;
			}

		} else {    // ICM20948
			select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H));

			if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report,
						 sizeof(icm_report))) {
				perf_end(_sample_perf);
				return;
			}
		}

		check_registers();

		if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) {
			return;
		}
	}

	/*
	 * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
	 * try to read a magnetometer report.
	 */

#   ifdef USE_I2C

	if (_mag->is_passthrough()) {
#   endif

		_mag->_measure(mpu_report.mag);

#   ifdef USE_I2C

	} else {
		_mag->measure();
	}

#   endif

	/*
	 * Continue evaluating gyro and accelerometer results
	 */
	if (!_magnetometer_only && _register_wait == 0) {

		/*
		 * Convert from big to little endian
		 */
		if (_whoami == ICM_WHOAMI_20948) {
			report.accel_x = int16_t_from_bytes(icm_report.accel_x);
			report.accel_y = int16_t_from_bytes(icm_report.accel_y);
			report.accel_z = int16_t_from_bytes(icm_report.accel_z);
			report.temp    = int16_t_from_bytes(icm_report.temp);
			report.gyro_x  = int16_t_from_bytes(icm_report.gyro_x);
			report.gyro_y  = int16_t_from_bytes(icm_report.gyro_y);
			report.gyro_z  = int16_t_from_bytes(icm_report.gyro_z);

		} else { // MPU9250/MPU6500
			report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
			report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
			report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
			report.temp    = int16_t_from_bytes(mpu_report.temp);
			report.gyro_x  = int16_t_from_bytes(mpu_report.gyro_x);
			report.gyro_y  = int16_t_from_bytes(mpu_report.gyro_y);
			report.gyro_z  = int16_t_from_bytes(mpu_report.gyro_z);
		}

		if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
			return;
		}
	}

	if (_register_wait != 0) {
		/*
		 * We are waiting for some good transfers before using the sensor again.
		 * We still increment _good_transfers, but don't return any data yet.
		 *
		*/
		_register_wait--;
		return;
	}

	/*
	 * Get sensor temperature
	 */
	_last_temperature = (report.temp) / 333.87f + 21.0f;


	/*
	 * Convert and publish accelerometer and gyrometer data.
	 */

	if (!_magnetometer_only) {

		/*
		 * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation
		 */
		if (_whoami != ICM_WHOAMI_20948) {
			/*
			 * Swap axes and negate y
			 */

			int16_t accel_xt = report.accel_y;
			int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);

			int16_t gyro_xt = report.gyro_y;
			int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);

			/*
			 * Apply the swap
			 */
			report.accel_x = accel_xt;
			report.accel_y = accel_yt;
			report.gyro_x = gyro_xt;
			report.gyro_y = gyro_yt;
		}

		/*
		 * Report buffers.
		 */
		sensor_accel_s		arb;
		sensor_gyro_s			grb;

		/*
		 * Adjust and scale results to m/s^2.
		 */
		grb.timestamp = arb.timestamp = hrt_absolute_time();

		// report the error count as the sum of the number of bad
		// transfers and bad register reads. This allows the higher
		// level code to decide if it should use this sensor based on
		// whether it has had failures
		grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

		/*
		 * 1) Scale raw value to SI units using scaling from datasheet.
		 * 2) Subtract static offset (in SI units)
		 * 3) Scale the statically calibrated values with a linear
		 *    dynamically obtained factor
		 *
		 * Note: the static sensor offset is the number the sensor outputs
		 * 	 at a nominally 'zero' input. Therefore the offset has to
		 * 	 be subtracted.
		 *
		 *	 Example: A gyro outputs a value of 74 at zero angular rate
		 *	 	  the offset is 74 from the origin and subtracting
		 *		  74 from all measurements centers them around zero.
		 */

		/* NOTE: Axes have been swapped to match the board a few lines above. */

		arb.x_raw = report.accel_x;
		arb.y_raw = report.accel_y;
		arb.z_raw = report.accel_z;

		float xraw_f = report.accel_x;
		float yraw_f = report.accel_y;
		float zraw_f = report.accel_z;

		// apply user specified rotation
		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

		float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
		float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
		float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

		arb.x = _accel_filter_x.apply(x_in_new);
		arb.y = _accel_filter_y.apply(y_in_new);
		arb.z = _accel_filter_z.apply(z_in_new);

		matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
		matrix::Vector3f aval_integrated;

		bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
		arb.x_integral = aval_integrated(0);
		arb.y_integral = aval_integrated(1);
		arb.z_integral = aval_integrated(2);

		arb.scaling = _accel_range_scale;

		arb.temperature = _last_temperature;

		/* return device ID */
		arb.device_id = _accel->_device_id.devid;

		grb.x_raw = report.gyro_x;
		grb.y_raw = report.gyro_y;
		grb.z_raw = report.gyro_z;

		xraw_f = report.gyro_x;
		yraw_f = report.gyro_y;
		zraw_f = report.gyro_z;

		// apply user specified rotation
		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

		float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
		float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
		float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

		grb.x = _gyro_filter_x.apply(x_gyro_in_new);
		grb.y = _gyro_filter_y.apply(y_gyro_in_new);
		grb.z = _gyro_filter_z.apply(z_gyro_in_new);

		matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
		matrix::Vector3f gval_integrated;

		bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
		grb.x_integral = gval_integrated(0);
		grb.y_integral = gval_integrated(1);
		grb.z_integral = gval_integrated(2);

		grb.scaling = _gyro_range_scale;

		grb.temperature = _last_temperature;

		/* return device ID */
		grb.device_id = _gyro->_device_id.devid;

		_accel_reports->force(&arb);
		_gyro_reports->force(&grb);

		/* notify anyone waiting for data */
		if (accel_notify) {
			_accel->poll_notify(POLLIN);
		}

		if (gyro_notify) {
			_gyro->parent_poll_notify();
		}

		if (accel_notify && !(_accel->_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
		}

		if (gyro_notify && !(_gyro->_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
		}
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
Пример #12
0
/////////////////////////////////////////////////////////
// main testing function 
/////////////////////////////////////////////////////////
int main(int argc, const char * const argv[])
{
  (void)argc;
  (void)argv;

  int coreid;
  int it;

  boolean_T pass;
  boolean_T flag;
  float y[10];
  int ix;
  float b_y;
  int b_k;
  float xbar;
  float r;
  float c_y;
  float check[2];
  float golden[4];



  /////////////////////////////////////////////////////////
  // main test loop 
  // each core loops over a kernel instance
  /////////////////////////////////////////////////////////
  coreid = get_core_id();

  printf("starting %d kernel iterations... (coreid = %d)\n",KERNEL_ITS,coreid);

  if (coreid>3)
    coreid=coreid-4;

  synch_barrier();

  perf_begin();

  for(it = 0; it < getKernelIts(); it++)
  {
    // matlab kernel
    for (ix = 0; ix < 10; ix++) {
      b_y = 0.0F;
      for (b_k = 0; b_k < 10; b_k++) {
        b_y += fv1[(ix + 10 * b_k) + 100 * coreid] * fv0[b_k + 10 * coreid];
      }
      y[ix] = b_y + fv3[coreid] * fv2[ix + 10 * coreid];
    }
  }

  synch_barrier();

  perf_end();

  synch_barrier();

  /////////////////////////////////////////////////////////
  // check results
  /////////////////////////////////////////////////////////

  b_y = y[0];
  ix = 0;
  xbar = y[0];
  for (b_k = 0; b_k < 9; b_k++) {
    b_y += y[b_k + 1];
    ix++;
    xbar += y[ix];
  }

  xbar *= 1.0F/10.0F;
  ix = 0;
  r = y[0] - xbar;
  c_y = r * r;
  for (b_k = 0; b_k < 9; b_k++) {
    ix++;
    r = y[ix] - xbar;
    c_y += r * r;
  }

  c_y *= 1.0F/9.0F;
  check[0] = b_y;
  check[1] = c_y;
  pass = true;
  for (ix = 0; ix < 2; ix++) {
    for (b_k = 0; b_k < 2; b_k++) {
      golden[b_k + (ix << 1)] = fv4[(b_k + (ix << 1)) + (coreid << 2)];
    }
    flag = true;
    flag = pass && (check[ix] <= golden[ix << 1]);
    flag = pass && (check[ix] >= golden[1 + (ix << 1)]);
    printErrors(!flag, ix, check[ix], golden[ix<<1], golden[1+(ix<<1)]);
    pass = pass && flag;
  }
  flagPassFail(pass, get_core_id());


/////////////////////////////////////////////////////////
// synchronize and exit
/////////////////////////////////////////////////////////

  return !pass;
}