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; } }