static void uavtalk_handle_msg(struct uavtalk_message *msg)
{
mavlink_message_t mav_msg;
switch (msg->objid) {
case UAVTALK_OBJID_ATTITUDESTATE:
mavlink_msg_attitude_pack(1, 1, &mav_msg, 0,
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_ROLL)),
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_PITCH)),
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_YAW)),
0, 0, 0);
mavlink_handle_msg(255, &mav_msg, NULL);
break;
case UAVTALK_OBJID_MANUALCONTROLCOMMAND:
case UAVTALK_OBJID_MANUALCONTROLCOMMAND_001:
case UAVTALK_OBJID_MANUALCONTROLCOMMAND_002:
mavlink_msg_rc_channels_raw_pack(1, 1, &mav_msg, 0, 0,
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_1),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_2),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_3),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_4),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_5),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_6),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_7),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_8),
0);
mavlink_handle_msg(255, &mav_msg, NULL);
break;
}
}
void mavlinkSendRCChannelsAndRSSI(void)
{
uint16_t msgLength;
mavlink_msg_rc_channels_raw_pack(0, 200, &mavMsg,
// time_boot_ms Timestamp (milliseconds since system boot)
millis(),
// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
0,
// chan1_raw RC channel 1 value, in microseconds
(rxRuntimeConfig.channelCount >= 1) ? rcData[0] : 0,
// chan2_raw RC channel 2 value, in microseconds
(rxRuntimeConfig.channelCount >= 2) ? rcData[1] : 0,
// chan3_raw RC channel 3 value, in microseconds
(rxRuntimeConfig.channelCount >= 3) ? rcData[2] : 0,
// chan4_raw RC channel 4 value, in microseconds
(rxRuntimeConfig.channelCount >= 4) ? rcData[3] : 0,
// chan5_raw RC channel 5 value, in microseconds
(rxRuntimeConfig.channelCount >= 5) ? rcData[4] : 0,
// chan6_raw RC channel 6 value, in microseconds
(rxRuntimeConfig.channelCount >= 6) ? rcData[5] : 0,
// chan7_raw RC channel 7 value, in microseconds
(rxRuntimeConfig.channelCount >= 7) ? rcData[6] : 0,
// chan8_raw RC channel 8 value, in microseconds
(rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
scaleRange(rssi, 0, 1023, 0, 255));
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
void MavLinkSendRcData(void)
{
mavlink_message_t msg;
mavlink_msg_rc_channels_raw_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
systemStatus.time*10,
0, rcSignalRudder, rcSignalThrottle, rcSignalMode, rcSignalTrack, 0, 0, 0, 0,
255);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
uint16_t PackRawRC(uint8_t system_id, uint8_t component_id, mavlink_rc_channels_raw_t mlRC_Commands ,uint32_t time_usec){
mavlink_system_t mavlink_system;
// if (!(mlPending.wpProtState == WP_PROT_IDLE))
// return 0;
mavlink_system.sysid = system_id; ///< ID 20 for this airplane
mavlink_system.compid = component_id;//MAV_COMP_ID_IMU; ///< The component sending the message is the IMU, it could be also a Linux process
//////////////////////////////////////////////////////////////////////////
mavlink_message_t msg;
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_rc_channels_raw_pack(mavlink_system.sysid, mavlink_system.compid, &msg , time_usec , mlRC_Commands.port, mlRC_Commands.chan1_raw, mlRC_Commands.chan2_raw, mlRC_Commands.chan3_raw, mlRC_Commands.chan4_raw, mlRC_Commands.chan5_raw, mlRC_Commands.chan6_raw, mlRC_Commands.chan7_raw, mlRC_Commands.chan8_raw,mlRC_Commands.rssi );
return( mavlink_msg_to_send_buffer(UartOutBuff, &msg));
}
void cDataLink::SendRcChannelsRaw(uint16_t ped_us, uint16_t col_us, uint16_t lon_us,
uint16_t lat_us, uint16_t aux_us,uint16_t mode_us)
{
int bytes_sent;
uint16_t len;
/* Send RC channels */
{
mavlink_msg_rc_channels_raw_pack(1, MAV_COMP_ID_ALL, &m_msg, microsSinceEpoch(),
uint8_t(1), // port
ped_us, col_us, lon_us, lat_us, //1...4
aux_us, mode_us, //5, 6
uint16_t(65535), uint16_t(65535), // 6...8, 65535 implies the channel is unused
uint8_t(255)); // rssi = unknown
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
}
void writeMavlink()
{
int systemId = 250;
int componentId = 0;
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_rc_channels_raw_pack
(systemId, componentId, &msg,
millis(), 8,
rcValue[0],
rcValue[1],
rcValue[2],
rcValue[3],
rcValue[4],
rcValue[5],
rcValue[6],
rcValue[7],
255);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
Serial1.write(buf, len);
}
task_return_t remote_send_raw(const remote_t* remote)
{
mavlink_message_t msg;
mavlink_msg_rc_channels_raw_pack( remote->mavlink_stream->sysid,
remote->mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
0,
remote->sat->channels[0] + 1024,
remote->sat->channels[1] + 1024,
remote->sat->channels[2] + 1024,
remote->sat->channels[3] + 1024,
remote->sat->channels[4] + 1024,
remote->sat->channels[5] + 1024,
remote->sat->channels[6] + 1024,
remote->sat->channels[7] + 1024,
// remote->mode.current_desired_mode.byte);
remote->signal_quality );
mavlink_stream_send(remote->mavlink_stream, &msg);
return TASK_RUN_SUCCESS;
}
static void uavoMavlinkBridgeTask(void *parameters) {
uint32_t lastSysTime;
// Main task loop
lastSysTime = PIOS_Thread_Systime();
FlightBatterySettingsData batSettings = {};
if (FlightBatterySettingsHandle() != NULL )
FlightBatterySettingsGet(&batSettings);
SystemStatsData systemStats;
while (1) {
PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ);
if (stream_trigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
FlightBatteryStateData batState = {};
if (FlightBatteryStateHandle() != NULL )
FlightBatteryStateGet(&batState);
SystemStatsGet(&systemStats);
int8_t battery_remaining = 0;
if (batSettings.Capacity != 0) {
if (batState.ConsumedEnergy < batSettings.Capacity) {
battery_remaining = 100 - lroundf(batState.ConsumedEnergy / batSettings.Capacity * 100);
}
}
uint16_t voltage = 0;
if (batSettings.VoltagePin != FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE)
voltage = lroundf(batState.Voltage * 1000);
uint16_t current = 0;
if (batSettings.CurrentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
current = lroundf(batState.Current * 100);
mavlink_msg_sys_status_pack(0, 200, mav_msg,
// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
(uint16_t)systemStats.CPULoad * 10,
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
voltage,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
current,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
battery_remaining,
// drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_count1 Autopilot-specific errors
0,
// errors_count2 Autopilot-specific errors
0,
// errors_count3 Autopilot-specific errors
0,
// errors_count4 Autopilot-specific errors
0);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_RC_CHANNELS)) {
ManualControlCommandData manualState;
FlightStatusData flightStatus;
ManualControlCommandGet(&manualState);
FlightStatusGet(&flightStatus);
SystemStatsGet(&systemStats);
//TODO connect with RSSI object and pass in last argument
mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
0,
// chan1_raw RC channel 1 value, in microseconds
manualState.Channel[0],
// chan2_raw RC channel 2 value, in microseconds
manualState.Channel[1],
// chan3_raw RC channel 3 value, in microseconds
manualState.Channel[2],
// chan4_raw RC channel 4 value, in microseconds
manualState.Channel[3],
// chan5_raw RC channel 5 value, in microseconds
manualState.Channel[4],
// chan6_raw RC channel 6 value, in microseconds
manualState.Channel[5],
// chan7_raw RC channel 7 value, in microseconds
manualState.Channel[6],
// chan8_raw RC channel 8 value, in microseconds
manualState.Channel[7],
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
manualState.Rssi);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_POSITION)) {
GPSPositionData gpsPosData = {};
HomeLocationData homeLocation = {};
SystemStatsGet(&systemStats);
if (GPSPositionHandle() != NULL )
GPSPositionGet(&gpsPosData);
if (HomeLocationHandle() != NULL )
HomeLocationGet(&homeLocation);
SystemStatsGet(&systemStats);
uint8_t gps_fix_type;
switch (gpsPosData.Status)
{
case GPSPOSITION_STATUS_NOGPS:
gps_fix_type = 0;
break;
case GPSPOSITION_STATUS_NOFIX:
gps_fix_type = 1;
break;
case GPSPOSITION_STATUS_FIX2D:
gps_fix_type = 2;
break;
case GPSPOSITION_STATUS_FIX3D:
case GPSPOSITION_STATUS_DIFF3D:
gps_fix_type = 3;
break;
default:
gps_fix_type = 0;
break;
}
mavlink_msg_gps_raw_int_pack(0, 200, mav_msg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
(uint64_t)systemStats.FlightTime * 1000,
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gps_fix_type,
// lat Latitude in 1E7 degrees
gpsPosData.Latitude,
// lon Longitude in 1E7 degrees
gpsPosData.Longitude,
// alt Altitude in 1E3 meters (millimeters) above MSL
gpsPosData.Altitude * 1000,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.HDOP * 100,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.VDOP * 100,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
gpsPosData.Groundspeed * 100,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
gpsPosData.Heading * 100,
// satellites_visible Number of satellites visible. If unknown, set to 255
gpsPosData.Satellites);
send_message();
mavlink_msg_gps_global_origin_pack(0, 200, mav_msg,
// latitude Latitude (WGS84), expressed as * 1E7
homeLocation.Latitude,
// longitude Longitude (WGS84), expressed as * 1E7
homeLocation.Longitude,
// altitude Altitude(WGS84), expressed as * 1000
homeLocation.Altitude * 1000);
send_message();
//TODO add waypoint nav stuff
//wp_target_bearing
//wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
//alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
//aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
//xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
//mavlink_msg_nav_controller_output_pack
//wp_number
//mavlink_msg_mission_current_pack
}
if (stream_trigger(MAV_DATA_STREAM_EXTRA1)) {
AttitudeActualData attActual;
SystemStatsData systemStats;
AttitudeActualGet(&attActual);
SystemStatsGet(&systemStats);
mavlink_msg_attitude_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// roll Roll angle (rad)
attActual.Roll * DEG2RAD,
// pitch Pitch angle (rad)
attActual.Pitch * DEG2RAD,
// yaw Yaw angle (rad)
attActual.Yaw * DEG2RAD,
// rollspeed Roll angular speed (rad/s)
0,
// pitchspeed Pitch angular speed (rad/s)
0,
// yawspeed Yaw angular speed (rad/s)
0);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_EXTRA2)) {
ActuatorDesiredData actDesired;
AttitudeActualData attActual;
AirspeedActualData airspeedActual = {};
GPSPositionData gpsPosData = {};
BaroAltitudeData baroAltitude = {};
FlightStatusData flightStatus;
if (AirspeedActualHandle() != NULL )
AirspeedActualGet(&airspeedActual);
if (GPSPositionHandle() != NULL )
GPSPositionGet(&gpsPosData);
if (BaroAltitudeHandle() != NULL )
BaroAltitudeGet(&baroAltitude);
ActuatorDesiredGet(&actDesired);
AttitudeActualGet(&attActual);
FlightStatusGet(&flightStatus);
float altitude = 0;
if (BaroAltitudeHandle() != NULL)
altitude = baroAltitude.Altitude;
else if (GPSPositionHandle() != NULL)
altitude = gpsPosData.Altitude;
// round attActual.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360)
int16_t heading = lroundf(attActual.Yaw);
if (heading < 0)
heading += 360;
mavlink_msg_vfr_hud_pack(0, 200, mav_msg,
// airspeed Current airspeed in m/s
airspeedActual.TrueAirspeed,
// groundspeed Current ground speed in m/s
gpsPosData.Groundspeed,
// heading Current heading in degrees, in compass units (0..360, 0=north)
heading,
// throttle Current throttle setting in integer percent, 0 to 100
actDesired.Throttle * 100,
// alt Current altitude (MSL), in meters
altitude,
// climb Current climb rate in meters/second
0);
send_message();
uint8_t armed_mode = 0;
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED)
armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t custom_mode = CUSTOM_MODE_STAB;
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
case FLIGHTSTATUS_FLIGHTMODE_MWRATE:
case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR:
case FLIGHTSTATUS_FLIGHTMODE_HORIZON:
/* Kinda a catch all */
custom_mode = CUSTOM_MODE_SPORT;
break;
case FLIGHTSTATUS_FLIGHTMODE_ACRO:
case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK:
custom_mode = CUSTOM_MODE_ACRO;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
/* May want these three to try and
* infer based on roll axis */
case FLIGHTSTATUS_FLIGHTMODE_LEVELING:
custom_mode = CUSTOM_MODE_STAB;
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
custom_mode = CUSTOM_MODE_DRIFT;
break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
custom_mode = CUSTOM_MODE_ALTH;
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
custom_mode = CUSTOM_MODE_RTL;
break;
case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
custom_mode = CUSTOM_MODE_POSH;
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
custom_mode = CUSTOM_MODE_AUTO;
break;
}
mavlink_msg_heartbeat_pack(0, 200, mav_msg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
MAV_TYPE_GENERIC,
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
MAV_AUTOPILOT_GENERIC,
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
armed_mode,
// custom_mode A bitfield for use for autopilot-specific flags.
custom_mode,
// system_status System status flag, see MAV_STATE ENUM
0);
send_message();
}
}
}
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;
}
}
