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, ¶m_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;
}
}