Exemple #1
0
void scheduleData (unsigned char hilOn, unsigned char* dataOut){
	
	// Generic message container used to pack the messages
	mavlink_message_t msg;
	
	// Generic buffer used to hold data to be streamed via serial port
	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
	
	// Cycles from 1 to 10 to decide which 
	// message's turn is to be sent
	static uint8_t samplePeriod = 1;
	
	// Contains the total bytes to send via the serial port
	uint8_t bytes2Send = 0;
	
	memset(&msg,0,sizeof(mavlink_message_t));
		
	switch (samplePeriod){
		case 1: //GPS 
			mavlink_msg_heartbeat_pack(SLUGS_SYSTEMID, 
																 SLUGS_COMPID, 
																 &msg, 
																 MAV_FIXED_WING, 
																 MAV_AUTOPILOT_SLUGS);
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
			
			memset(&msg,0,sizeof(mavlink_message_t));
			
			// Pack the GPS message
			mavlink_msg_gps_raw_pack(SLUGS_SYSTEMID, 
																 SLUGS_COMPID, 
																 &msg, 
																 mlGpsData.usec, 
																 mlGpsData.fix_type, 
																 mlGpsData.lat, 
																 mlGpsData.lon, 
																 mlGpsData.alt, 
																 mlGpsData.eph, 
																 mlGpsData.epv, 
																 mlGpsData.v, 
																 mlGpsData.hdg);															 
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg); 

		break;
		
		case 2: // LOAD 
			mavlink_msg_cpu_load_encode( SLUGS_SYSTEMID, 
														 			 SLUGS_COMPID, 
														 			 &msg, 
														 			 &mlCpuLoadData);
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
			
		break;
		case 3:	// XYZ 		
			mavlink_msg_local_position_encode( SLUGS_SYSTEMID, 
														 			 			 SLUGS_COMPID, 
														 			 			 &msg, 
														 			 			 &mlLocalPositionData);
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
				
		break;
		case 4:	// Dynamic and Reboot (if required)		
			mavlink_msg_scaled_pressure_encode( SLUGS_SYSTEMID, 
														 			 				SLUGS_COMPID, 
														 			 				&msg, 
														 			 				&mlAirData);
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);	
			
			// clear the message
				memset(&msg,0,sizeof(mavlink_message_t));
				
			// it there has been a reboot
			if(mlBoot.version == 1){
			 // Copy the message to the send buffer
			 mavlink_msg_boot_pack(SLUGS_SYSTEMID, 
														 SLUGS_COMPID, 
														 &msg, 
														 1);
			 mlBoot.version=0;
		  }
		  
		  bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
		
		break;

		case 5:	// Bias		
			mavlink_msg_sensor_bias_encode( SLUGS_SYSTEMID, 
														 					SLUGS_COMPID, 
														 					&msg, 
														 					&mlSensorBiasData);	
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
							
		break;
		case 6: // Diagnostic
			mavlink_msg_diagnostic_encode( SLUGS_SYSTEMID, 
																   	 SLUGS_COMPID, 
																   	 &msg, 
																   	 &mlDiagnosticData);
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);	
					
		break;
		
		case 7: // Pilot Console Data
			mavlink_msg_rc_channels_raw_encode( SLUGS_SYSTEMID, 
														 						SLUGS_COMPID, 
														 						&msg, 
														 						&mlPilotConsoleData);
														 						
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
				
		break;
		
		case 8: // Sensor Data in meaningful units
				mavlink_msg_scaled_imu_encode( SLUGS_SYSTEMID, 
														   					SLUGS_COMPID, 
														   					&msg, 
														   					&mlFilteredData);
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);	
		
		break;
		
		case 9: // Raw Pressure
					mavlink_msg_raw_pressure_encode( SLUGS_SYSTEMID, 
																 					SLUGS_COMPID, 
																 					&msg, 
																	 				&mlRawPressureData);
			// Copy the message to the send buffer
			bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);	
		
		break;
		default:
			*dataOut = 0;
		
		break;
	}
	
	memset(&msg,0,sizeof(mavlink_message_t));
	
	// Attitude data. Gets included every sample time
	mavlink_msg_attitude_encode( SLUGS_SYSTEMID, 
														 	 SLUGS_COMPID, 
														 	 &msg, 
														 	 &mlAttitudeData);
	
	// Copy the message to the send buffer	
	bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
	
	memset(&msg,0,sizeof(mavlink_message_t));

	// Sensor Raw data. Gets included every sample time
	mavlink_msg_raw_imu_encode( SLUGS_SYSTEMID, 
														SLUGS_COMPID, 
														&msg, 
														&mlRawImuData);

	bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);

	// Put the length of the message in the first byte of the outgoing array
	*dataOut = bytes2Send;
		
	// increment/overflow the samplePeriod counter
	// configured for 10 Hz in non vital messages
	samplePeriod = (samplePeriod >= 10)? 1: samplePeriod + 1;
	
	// Send the data via the debug serial port
	
	if (hilOn == 0){	
		send2DebugPort(dataOut, hilOn);
	}
}
void MAVLinkMKHUCHApp::handle_input(const mkhuch_message_t& msg) {
	uint64_t last_mkhuch_msg_time = mkhuch_msg_time;
	mkhuch_msg_time = get_time_us();

	log("MAVLinkMKHUCHApp got mkhuch_message", static_cast<int>(msg.type), Logger::LOGLEVEL_DEBUG);

	//send heartbeat approx. after 1s
	heartbeat_time += mkhuch_msg_time - last_mkhuch_msg_time;
	if( heartbeat_time > 1000000 ) {
		send_heartbeat();
		heartbeat_time = 0;
	}

	switch(msg.type) {
		case MKHUCH_MSG_TYPE_MK_IMU: {
			const mkhuch_mk_imu_t *mk_imu = reinterpret_cast<const mkhuch_mk_imu_t*>(msg.data);
			mavlink_huch_imu_raw_adc_t imu_raw_adc;
			imu_raw_adc.xacc = mk_imu->x_adc_acc;
			imu_raw_adc.yacc = mk_imu->y_adc_acc;
			imu_raw_adc.zacc = mk_imu->z_adc_acc;
			imu_raw_adc.xgyro = mk_imu->x_adc_gyro;
			imu_raw_adc.ygyro = mk_imu->y_adc_gyro;
			imu_raw_adc.zgyro = mk_imu->z_adc_gyro;
			DataCenter::set_huch_imu_raw_adc(imu_raw_adc);
			Lock tx_lock(tx_mav_mutex);
			//forward raw ADC
			mavlink_msg_huch_imu_raw_adc_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&imu_raw_adc
				);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			//forward MK IMU
			//TODO: add compass value and baro
			mavlink_huch_mk_imu_t huch_mk_imu;
			huch_mk_imu.usec = mkhuch_msg_time;
			huch_mk_imu.xacc = (2500*mk_imu->x_acc)/1024; //convert normalized analog to mg
			huch_mk_imu.yacc = (2500*mk_imu->y_acc)/1024;
			huch_mk_imu.zacc = (2500*mk_imu->z_acc)/1024;
			huch_mk_imu.xgyro = (6700*mk_imu->x_adc_gyro)/(3*1024); //convert analog to 0.1 deg./sec.
			huch_mk_imu.ygyro = (6700*mk_imu->y_adc_gyro)/(3*1024);
			huch_mk_imu.zgyro = (6700*mk_imu->z_adc_gyro)/(3*1024);
			DataCenter::set_huch_mk_imu(huch_mk_imu);
			mavlink_msg_huch_mk_imu_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&huch_mk_imu
				);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			//forward pressure
			mavlink_raw_pressure_t raw_pressure;
			raw_pressure.time_usec = mkhuch_msg_time;
			raw_pressure.press_abs = mk_imu->press_abs;
			raw_pressure.press_diff1 = 0;	//press_diff1
			raw_pressure.press_diff2 = 0;	//press_diff2
			raw_pressure.temperature = 0;	//temperature
			DataCenter::set_raw_pressure(raw_pressure);
			mavlink_msg_raw_pressure_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&raw_pressure
				);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			//TODO: forward magneto
			break;
		}
/*		case MKHUCH_MSG_TYPE_PARAM_VALUE: {
			const mkhuch_param_t *param = reinterpret_cast<const mkhuch_param_t*>(msg.data);
			//set parameter
			uint8_t index;
			if(param->index >= parameter_count)
				index = parameter_count-1;
			else
				index = param->index;
			parameters[index] = param->value;
			//ask for next parameter
			if(index < parameter_count - 1) {
				parameter_request = index + 1;
				mk_param_type_t param_type= static_cast<mk_param_type_t>(parameter_request);
				send(MKHUCH_MSG_TYPE_PARAM_REQUEST, &param_type, sizeof(mk_param_type_t));
				parameter_time = message_time;
			} else { //got all parameters
				parameter_request = 255;
			}
			//inform others
			send_mavlink_param_value( static_cast<mk_param_type_t>(index) );
			break;
		}*/
/*		case MKHUCH_MSG_TYPE_ACTION_ACK: {

			Lock tx_lock(tx_mav_mutex);
			mavlink_msg_action_ack_pack(owner()->system_id(), component_id, &tx_mav_msg, msg.data[0], msg.data[1]);
			send(tx_mav_msg);
			break;
		}*/
	case MKHUCH_MSG_TYPE_STICKS: {
		const mkhuch_sticks_t *sticks = reinterpret_cast<const mkhuch_sticks_t*>(msg.data);
		mavlink_huch_attitude_control_t attitude_control;
		attitude_control.roll = (float)sticks->roll;
		attitude_control.pitch = (float)sticks->pitch;
		attitude_control.yaw = (float)sticks->yaw;
		attitude_control.thrust = (float)sticks->thrust;
		attitude_control.target = system_id();
		attitude_control.mask = 0;
		// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->roll), static_cast<int16_t>(sticks->pitch), Logger::LOGLEVEL_DEBUG);
		// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->yaw), static_cast<int16_t>(sticks->thrust), Logger::LOGLEVEL_DEBUG);
		// ALERT uint64_t -> uint32_t cast
		uint32_t time_ms = get_time_ms();
		Lock tx_lock(tx_mav_mutex);
		mavlink_msg_huch_attitude_control_encode(
																					 system_id(),
																					 component_id,
																					 &tx_mav_msg,
																					 &attitude_control
																					 );
		AppLayer<mavlink_message_t>::send(tx_mav_msg);

		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_roll",
		// 	sticks->roll);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_pitch",
		// 	sticks->pitch);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_yaw",
		// 	sticks->yaw);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_thrust",
		// 	sticks->thrust);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		break;
	}

	case MKHUCH_MSG_TYPE_RC_CHANNELS_RAW: {
		const mkhuch_rc_channels_raw_t *rc_channels_raw = reinterpret_cast<const mkhuch_rc_channels_raw_t*>(msg.data);
		//log("MAVLinkMKHUCHApp got mkhuch_rc_channels_raw msg", static_cast<int16_t>(rc_channels_raw->channel_2), Logger::LOGLEVEL_DEBUG);
		Lock tx_lock(tx_mav_mutex);
		mavlink_msg_rc_channels_raw_pack(system_id(),
			component_id,
			&tx_mav_msg,
			get_time_ms(),
			0,	//FIXME
			rc_channels_raw->channel_1,
			rc_channels_raw->channel_2,
			rc_channels_raw->channel_3,
			rc_channels_raw->channel_4,
			rc_channels_raw->channel_5,
			rc_channels_raw->channel_6,
			rc_channels_raw->channel_7,
			rc_channels_raw->channel_8,
			rc_channels_raw->rssi);
		AppLayer<mavlink_message_t>::send(tx_mav_msg);
		break;
	}

	case MKHUCH_MSG_TYPE_SYSTEM_STATUS: {
/* FIXME
			const mkhuch_system_status_t *sys_status = reinterpret_cast<const mkhuch_system_status_t*>(msg.data);
			Lock tx_lock(tx_mav_mutex);
			mavlink_msg_sys_status_pack(system_id(),
				component_id,
				&tx_mav_msg,
				sys_status->mode,
				sys_status->nav_mode,
				sys_status->state,
				1000, //FIXME: use glibtop to get load of linux system
				sys_status->vbat*100, //convert dV to mV
				0,//motor block (unsupported)
				sys_status->packet_drop);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
*/
			break;
		}
/*		case MKHUCH_MSG_TYPE_BOOT:
			//TODO
			break;*/
		case MKHUCH_MSG_TYPE_ATTITUDE: {
			const mkhuch_attitude_t *mkhuch_attitude = reinterpret_cast<const mkhuch_attitude_t*>(msg.data);
			mavlink_attitude_t mavlink_attitude;
			mavlink_attitude.time_boot_ms = mkhuch_msg_time / 1000;
			mavlink_attitude.roll = 0.001745329251994329577*(mkhuch_attitude->roll_angle);		//convert cdeg to rad with (pi/180)/10 
			mavlink_attitude.pitch = 0.001745329251994329577*(mkhuch_attitude->pitch_angle);	//convert cdeg to rad with (pi/180)/10
			mavlink_attitude.yaw = 0.001745329251994329577*(mkhuch_attitude->yaw_angle);		//convert cdeg to rad with (pi/180)/10
			//FIXME
			mavlink_attitude.rollspeed = 0;
			mavlink_attitude.pitchspeed = 0;
			mavlink_attitude.yawspeed = 0;
			Lock tx_lock(tx_mav_mutex);
			mavlink_msg_attitude_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&mavlink_attitude);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			break;
		}
		default:
			break;
	}
}