void GCS_Mavlink<T>::send_attitude(mavlink_attitude_t &attitude)
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_attitude_encode(_systemId, MAV_COMP_ID_ALL, &msg, &attitude);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
void MAVLinkProxy::navdata()
{
static mavlink_message_t msg;
static uint8_t buf[MAVLINK_MAX_PACKET_LEN];
if(_attitude.time_boot_ms > 0)
{
mavlink_msg_attitude_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg, &_attitude);
mavlink_msg_to_send_buffer(buf, &msg);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
}
if(_gps.time_usec > 0)
{
mavlink_msg_gps_raw_int_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg, &_gps);
mavlink_msg_to_send_buffer(buf, &msg);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
}
_navdata_timer.expires_from_now(boost::posix_time::milliseconds(MAVLINK_NAVDATA_INTERVAL));
_navdata_timer.async_wait(boost::bind(&MAVLinkProxy::navdata, this));
}
void loop(void)
{
uint16_t err_count = 0;
// incoming heartbeat
mavlink_message_t msg;
mavlink_heartbeat_t heartbeat = {0};
mavlink_msg_heartbeat_encode(3, 1, &msg, &heartbeat);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("heartbeat should be processed locally\n");
err_count++;
}
// incoming non-targetted message
mavlink_attitude_t attitude = {0};
mavlink_msg_attitude_encode(3, 1, &msg, &attitude);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("attitude should be processed locally\n");
err_count++;
}
// incoming targeted message for someone else
mavlink_param_set_t param_set = {0};
param_set.target_system = mavlink_system.sysid+1;
param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 1 should not be processed locally\n");
err_count++;
}
// incoming targeted message for us
param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 2 should be processed locally\n");
err_count++;
}
// incoming targeted message for our system, but other compid
// should be processed locally
param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 3 should be processed locally\n");
err_count++;
}
// incoming broadcast message should be processed locally
param_set.target_system = 0;
param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 4 should be processed locally\n");
err_count++;
}
if (err_count == 0) {
hal.console->printf("All OK\n");
}
hal.scheduler->delay(1000);
}
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);
}
}
// ------------------------------------------------------------------------------
// Top
// ------------------------------------------------------------------------------
int
top (int argc, char **arv)
{
// --------------------------------------------------------------------------
// SETUP!!
// --------------------------------------------------------------------------
// sudo apt-get install socat (only once)
// sudo chown <user> /dev (only run once)
// socat -d -d pty,raw,echo=0,link=/dev/ttyVA00 pty,raw,echo=0,link=/dev/ttyUSB0
// or!!
// ./start_pseudo_mavlink.sh starts everthing
// --------------------------------------------------------------------------
// Start Port
// --------------------------------------------------------------------------
char *uart_name = (char*)"/dev/ttyVA00"; // pseudo teletype
int baudrate = 57600;
Serial_Port serial_port(uart_name,baudrate);
serial_port.start();
// OWNSHIP POSITION AND VELOCITY, LOCAL_NED
mavlink_local_position_ned_t lp;
lp.time_boot_ms = (uint32_t) (get_time_usec()/1000);
lp.x = 0.1;
lp.y = 0.2;
lp.z = 0.3;
lp.vx = 0.4;
lp.vy = 0.5;
lp.vz = 0.0;
mavlink_attitude_t att;
att.roll = 0;
att.pitch = 0;
att.yaw = 0;
att.rollspeed = 0;
att.pitchspeed = 0;
att.yawspeed = 0;
mavlink_heartbeat_t hb;
hb.custom_mode = 0;
hb.type = 0;
hb.autopilot = 0;
hb.base_mode = 0;
hb.system_status = 0;
hb.mavlink_version = 0;
while (1)
{
mavlink_message_t lp_m, att_m, hb_m;
mavlink_msg_local_position_ned_encode(system_id, component_id, &lp_m, &lp);
mavlink_msg_attitude_encode(system_id, component_id, &att_m, &att);
mavlink_msg_heartbeat_encode(system_id, component_id, &hb_m, &hb);
serial_port.write_message(lp_m);
serial_port.write_message(att_m);
serial_port.write_message(hb_m);
usleep(0.5e6); //2Hz
}
return 0;
}
/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent * ev)
{
UAVObjMetadata metadata;
// FlightTelemetryStatsData flightStats;
// GCSTelemetryStatsData gcsTelemetryStatsData;
// int32_t retries;
// int32_t success;
if (ev->obj == 0) {
updateTelemetryStats();
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else if (ev->obj == TelemetrySettingsHandle()) {
updateSettings();
} else {
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for
}
mavlink_message_t msg;
mavlink_system.sysid = 20;
mavlink_system.compid = MAV_COMP_ID_IMU;
mavlink_system.type = MAV_TYPE_FIXED_WING;
uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT;
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
// Setup type and object id fields
uint32_t objId = UAVObjGetID(ev->obj);
// uint64_t timeStamp = 0;
switch(objId) {
case BAROALTITUDE_OBJID:
{
BaroAltitudeGet(&baroAltitude);
pressure.press_abs = baroAltitude.Pressure*10.0f;
pressure.temperature = baroAltitude.Temperature*100.0f;
mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case FLIGHTTELEMETRYSTATS_OBJID:
{
// FlightTelemetryStatsData flightTelemetryStats;
FlightTelemetryStatsGet(&flightStats);
// XXX this is a hack to make it think it got a confirmed
// connection
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
GCSTelemetryStatsGet(&gcsTelemetryStatsData);
gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
//
//
// //mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass);
// mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case SYSTEMSTATS_OBJID:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t system_state = MAV_STATE_UNINIT;
uint8_t base_mode = 0;
uint8_t custom_mode = 0;
// Set flight mode
switch (flightStatus.FlightMode)
{
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
base_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
default:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
}
// Set arming state
switch (flightStatus.Armed)
{
case FLIGHTSTATUS_ARMED_ARMING:
case FLIGHTSTATUS_ARMED_ARMED:
system_state = MAV_STATE_ACTIVE;
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case FLIGHTSTATUS_ARMED_DISARMED:
system_state = MAV_STATE_STANDBY;
base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED;
break;
}
// Set HIL
if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state);
SystemStatsData stats;
SystemStatsGet(&stats);
FlightBatteryStateData flightBatteryData;
FlightBatteryStateGet(&flightBatteryData);
FlightBatterySettingsData flightBatterySettings;
FlightBatterySettingsGet(&flightBatterySettings);
uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f);
int16_t batteryCurrent = -1; // -1: Not present / not estimated
int8_t batteryPercent = -1; // -1: Not present / not estimated
// if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0)
// {
// Factor is zero, sensor is not present
// Estimate remaining capacity based on lipo curve
batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f));
// }
// else
// {
// // Use capacity and current
// batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity);
// batteryCurrent = flightBatteryData.Current*100;
// }
mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case ATTITUDERAW_OBJID:
{
AttitudeRawGet(&attitudeRaw);
// Copy data
attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X];
attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y];
attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z];
attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X];
attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y];
attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z];
attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X];
attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y];
attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z];
mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
if (hilEnabled)
{
mavlink_hil_controls_t controls;
// Copy data
controls.roll_ailerons = 0.1;
controls.pitch_elevator = 0.1;
controls.yaw_rudder = 0.0;
controls.throttle = 0.8;
mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls);
// Copy the message to the send buffer
len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
}
case ATTITUDEMATRIX_OBJID:
{
AttitudeMatrixGet(&attitudeMatrix);
// Copy data
attitude.roll = attitudeMatrix.Roll;
attitude.pitch = attitudeMatrix.Pitch;
attitude.yaw = attitudeMatrix.Yaw;
attitude.rollspeed = attitudeMatrix.AngularRates[0];
attitude.pitchspeed = attitudeMatrix.AngularRates[1];
attitude.yawspeed = attitudeMatrix.AngularRates[2];
mavlink_msg_attitude_encode(mavlink_system.sysid,
mavlink_system.compid, &msg, &attitude);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case GPSPOSITION_OBJID:
{
GPSPositionGet(&gpsPosition);
gps_raw.time_usec = 0;
gps_raw.lat = gpsPosition.Latitude*10;
gps_raw.lon = gpsPosition.Longitude*10;
gps_raw.alt = gpsPosition.Altitude*10;
gps_raw.eph = gpsPosition.HDOP*100;
gps_raw.epv = gpsPosition.VDOP*100;
gps_raw.cog = gpsPosition.Heading*100;
gps_raw.satellites_visible = gpsPosition.Satellites;
gps_raw.fix_type = gpsPosition.Status;
mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
// mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0);
break;
}
case POSITIONACTUAL_OBJID:
{
PositionActualData pos;
PositionActualGet(&pos);
mavlink_local_position_ned_t m_pos;
m_pos.time_boot_ms = 0;
m_pos.x = pos.North;
m_pos.y = pos.East;
m_pos.z = pos.Down;
m_pos.vx = 0.0f;
m_pos.vy = 0.0f;
m_pos.vz = 0.0f;
mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
case ACTUATORCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
ActuatorCommandData act;
ActuatorCommandGet(&act);
rc.chan5_scaled = act.Channel[0];
rc.chan6_scaled = act.Channel[1];
rc.chan7_scaled = act.Channel[2];
rc.chan8_scaled = act.Channel[3];
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
case MANUALCONTROLCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
rc.chan5_scaled = 0;
rc.chan6_scaled = 0;
rc.chan7_scaled = 0;
rc.chan8_scaled = 0;
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
default:
{
//printf("unknown object: %x\n",(unsigned int)objId);
break;
}
}
}
}
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;
}
}
