void PROTOCOL_Task( void *pvParameters )
{
mavlink_msg_heartbeat_send(0, global_data.param[PARAM_SYSTEM_TYPE], 0);
mavlink_msg_boot_send(0, global_data.param[PARAM_SW_VERSION]);
//mavlink_msg_statustext_send(0, 1, "FLYless");
while(1)
{
vTaskDelay( 100 / ONE_MS);
ADXL_Convert_to_G(&global_data.acc_raw,&global_data.acc_g);
mavlink_msg_heartbeat_send(0, global_data.param[PARAM_SYSTEM_TYPE], 0);
mavlink_msg_sys_status_send(0, global_data.mode, global_data.nav, global_data.state, 3000, 0, 0);
mavlink_msg_attitude_send(0, 100, global_data.attitude.x,global_data.attitude.y, global_data.attitude.z, global_data.gyro_rad.x, global_data.gyro_rad.y, global_data.gyro_rad.z);
mavlink_msg_raw_imu_send(0, 100, global_data.acc_g.x, global_data.acc_g.y, global_data.acc_g.z, global_data.gyro_raw.x, global_data.gyro_raw.y, global_data.gyro_raw.z, global_data.magnet_raw.x, global_data.magnet_raw.y, global_data.magnet_raw.z);
handle_mav_link_mess();
/* should not be here */
int8_t x1,x2,x3,x4;
x1 = (int8_t)global_data.param[8];
x2 = (int8_t)global_data.param[9];
x3 = (int8_t)global_data.param[10];
x4 = (int8_t)global_data.param[11];
SERVO_SetValue(x1,x2,x3,x4);
}
}
void send_system_state(void)
{
// Send heartbeat to announce presence of this system
// Send over both communication links
mavlink_msg_heartbeat_send(MAVLINK_COMM_1,
global_data.param[PARAM_SYSTEM_TYPE], MAV_AUTOPILOT_PIXHAWK);
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
global_data.param[PARAM_SYSTEM_TYPE], MAV_AUTOPILOT_PIXHAWK);
// Send system status over both links
mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
// Send auxiliary status over both links
mavlink_msg_aux_status_send(MAVLINK_COMM_1, global_data.cpu_usage,
global_data.i2c0_err_count, global_data.i2c1_err_count,
global_data.spi_err_count, global_data.spi_err_count,
communication_get_uart_drop_rate());
mavlink_msg_aux_status_send(MAVLINK_COMM_0, global_data.cpu_usage,
global_data.i2c0_err_count, global_data.i2c1_err_count,
global_data.spi_err_count, global_data.spi_err_count,
communication_get_uart_drop_rate());
// mavlink_msg_raw_aux_send(MAVLINK_COMM_0, 0, 0, 0, 0,
// battery_get_value(), global_data.temperature,
// global_data.pressure_raw);
}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
/* always send the heartbeat, independent of the update status of the topics */
if (!status_sub->update(&status)) {
/* if topic update failed fill it with defaults */
memset(&status, 0, sizeof(status));
}
if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
/* if topic update failed fill it with defaults */
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
}
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
void
l_vehicle_status(const struct listener *l)
{
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
if (v_status.hil_state == HIL_STATE_ON)
set_hil_on_off(true);
else if (v_status.hil_state == HIL_STATE_OFF)
set_hil_on_off(false);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
// local function definitions
static void mavlink_send_heartbeat(void)
{
MAV_MODE armed_mode = MAV_MODE_ENUM_END; // used for failsafe
if(_armed_state == ARMED)
armed_mode = MAV_MODE_MANUAL_ARMED;
else if(_armed_state == DISARMED)
armed_mode = MAV_MODE_MANUAL_DISARMED;
uint8_t control_mode = 0;
if(get_param_int(PARAM_FIXED_WING))
{
control_mode = MODE_PASS_THROUGH;
}
else if(rc_switch(get_param_int(PARAM_RC_F_CONTROL_TYPE_CHANNEL)))
{
control_mode = MODE_ROLL_PITCH_YAWRATE_ALTITUDE;
}
else
{
control_mode = rc_switch(get_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL)) ? MODE_ROLL_PITCH_YAWRATE_THROTTLE : MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE;
}
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
get_param_int(PARAM_FIXED_WING) ? MAV_TYPE_FIXED_WING : MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_GENERIC,
armed_mode,
control_mode,
MAV_STATE_STANDBY);
}
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
switch (control_mode) {
case AUTO:
case RTL:
case LOITER:
case GUIDED:
case CIRCLE:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
}
// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
#if HIL_MODE != HIL_MODE_DISABLED
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
#endif
// we are armed if we are not initialising
if (0){//motors.armed()) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
}
static void *heartbeatloop(void * arg)
{
/* initialize waypoint manager */
mavlink_wpm_init(&wpm);
/* initialize parameter storage */
mavlink_pm_reset_params(&pm);
int lowspeed_counter = 0;
int result_sys_status_trylock;
while(1) {
// sleep
usleep(50000);
// 1 Hz
if (lowspeed_counter == 10)
{
/* Send heartbeat */
mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_GENERIC,MAV_MODE_PREFLIGHT,custom_mode,MAV_STATE_UNINIT);
/* Send status */
result_sys_status_trylock = global_data_trylock(&global_data_sys_status.access_conf);
if(0 == result_sys_status_trylock) mavlink_msg_sys_status_send(chan, global_data_sys_status.onboard_control_sensors_present, global_data_sys_status.onboard_control_sensors_enabled, global_data_sys_status.onboard_control_sensors_health, global_data_sys_status.load, global_data_sys_status.voltage_battery, global_data_sys_status.current_battery, global_data_sys_status.battery_remaining, global_data_sys_status.drop_rate_comm, global_data_sys_status.errors_comm, global_data_sys_status.errors_count1, global_data_sys_status.errors_count2, global_data_sys_status.errors_count3, global_data_sys_status.errors_count4);
global_data_unlock(&global_data_sys_status.access_conf);
if (!(mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
// System is not armed, blink at 1 Hz
led_toggle(LED_BLUE);
}
lowspeed_counter = 0;
}
lowspeed_counter++;
// Send one parameter
mavlink_pm_queued_send();
if (mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED) {
// System is armed, blink at 10 Hz
led_toggle(LED_BLUE);
}
usleep(50000);
}
}
void send_heartbeat(mavlink_channel_t chan) {
uint8_t base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED
| MAV_MODE_FLAG_SAFETY_ARMED
| MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = 5 ; /* Loiter is mode 5. */
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
}
void Tracker::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
switch (control_mode) {
case MANUAL:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case STOP:
break;
case SCAN:
case SERVO_TEST:
case AUTO:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
case INITIALISING:
system_status = MAV_STATE_CALIBRATING;
break;
}
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_ANTENNA_TRACKER,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
}
void send(const hrt_abstime t)
{
(void)status_sub->update(t);
(void)pos_sp_triplet_sub->update(t);
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
void
l_vehicle_status(struct listener *l)
{
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
set_hil_on_off(v_status.flag_hil_enabled);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_mode,
v_status.state_machine,
mavlink_state);
}
void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
//_board->debug->printf_P(PSTR("send message\n"));
// if number of channels exceeded return
if (_channel == MAVLINK_COMM_3)
return;
uint64_t timeStamp = micros();
switch (id) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_msg_heartbeat_send(_channel, _board->getVehicle(),
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_msg_attitude_send(_channel, timeStamp,
_navigator->getRoll(), _navigator->getPitch(),
_navigator->getYaw(), _navigator->getRollRate(),
_navigator->getPitchRate(), _navigator->getYawRate());
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION: {
mavlink_msg_global_position_send(_channel, timeStamp,
_navigator->getLat() * rad2Deg,
_navigator->getLon() * rad2Deg, _navigator->getAlt(),
_navigator->getVN(), _navigator->getVE(),
_navigator->getVD());
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION: {
mavlink_msg_local_position_send(_channel, timeStamp,
_navigator->getPN(),_navigator->getPE(), _navigator->getPD(),
_navigator->getVN(), _navigator->getVE(), _navigator->getVD());
break;
}
case MAVLINK_MSG_ID_GPS_RAW: {
mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(),
_board->gps->latitude/1.0e7,
_board->gps->longitude/1.0e7, _board->gps->altitude/100.0, 0, 0,
_board->gps->ground_speed/100.0,
_board->gps->ground_course/10.0);
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(),
_board->gps->latitude,
_board->gps->longitude, _board->gps->altitude*10.0, 0, 0,
_board->gps->ground_speed/100.0,
_board->gps->ground_course/10.0);
break;
}
case MAVLINK_MSG_ID_SCALED_IMU: {
int16_t xmag, ymag, zmag;
xmag = ymag = zmag = 0;
if (_board->compass) {
// XXX THIS IS NOT SCALED
xmag = _board->compass->mag_x;
ymag = _board->compass->mag_y;
zmag = _board->compass->mag_z;
}
mavlink_msg_scaled_imu_send(_channel, timeStamp,
_navigator->getXAccel()*1e3,
_navigator->getYAccel()*1e3,
_navigator->getZAccel()*1e3,
_navigator->getRollRate()*1e3,
_navigator->getPitchRate()*1e3,
_navigator->getYawRate()*1e3,
xmag, ymag, zmag);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
int16_t ch[8];
for (int i = 0; i < 8; i++)
ch[i] = 0;
for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++) {
ch[i] = 10000 * _board->rc[i]->getPosition();
//_board->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]);
}
mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2],
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
int16_t ch[8];
for (int i = 0; i < 8; i++)
ch[i] = 0;
for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++)
ch[i] = _board->rc[i]->getPwm();
mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2],
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
uint16_t batteryVoltage = 0; // (milli volts)
uint16_t batteryPercentage = 1000; // times 10
if (_board->batteryMonitor) {
batteryPercentage = _board->batteryMonitor->getPercentage()*10;
batteryVoltage = _board->batteryMonitor->getVoltage()*1000;
}
mavlink_msg_sys_status_send(_channel, _controller->getMode(),
_guide->getMode(), _controller->getState(), _board->load * 10,
batteryVoltage, batteryPercentage, _packetDrops);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
//mavlink_waypoint_ack_t packet;
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
_cmdDestCompId, type);
// turn off waypoint send
_receivingCmds = false;
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
break;
}
default: {
char msg[50];
sprintf(msg, "autopilot sending unknown command with id: %d", id);
sendText(SEVERITY_HIGH, msg);
}
} // switch
} // send message
static void stabilizerTask(void* param)
{
uint32_t lastWakeTime;
//uint32_t tempTime;
uint16_t heartbCounter = 0;
uint16_t attitudeCounter = 0;
uint16_t altHoldCounter = 0;
//uint32_t data[6];
//Wait for the system to be fully started to start stabilization loop
systemWaitStart();
lastWakeTime = xTaskGetTickCount ();
for( ; ;)
{
//tempTime = lastWakeTime;
vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz
heartbCounter ++;
/*
if (lastWakeTime < tempTime) {
tempTime = (0 - tempTime) + lastWakeTime;
} else {
tempTime = lastWakeTime - tempTime;
}
*/
while (heartbCounter >= HEART_UPDATE_RATE_DIVIDER) { // 1Hz
MAVLINK(mavlink_msg_heartbeat_send(MAVLINK_COMM_0, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY);)
heartbCounter = 0;
}
imuRead(&gyro, &acc, &mag);
if (imu6IsCalibrated())
{
// 250HZ
if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)
{
MahonyAHRSupdateIMU(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z);
//filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z);
//MahonyAHRSupdate(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z);
//MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z);
//filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z);
//MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z);
sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);
radRollActual = eulerRollActual * M_PI / 180.0f;
radPitchActual = eulerPitchActual * M_PI / 180.0f;
radYawActual = eulerYawActual * M_PI / 180.0f;
//float yh, xh;
#define yh (mag.y * cos(radRollActual) - mag.z * sin(radRollActual))
#define xh (mag.x*cos(radPitchActual) + mag.y*sin(radRollActual)*sin(radPitchActual) + mag.z * cos(radRollActual)*sin(radPitchActual))
radYawActual = atan2(-yh,xh);
MAVLINK(mavlink_msg_attitude_send(MAVLINK_COMM_0, lastWakeTime, \
radRollActual, radPitchActual, radYawActual, \
gyro.x, gyro.y, gyro.z);)
attitudeCounter = 0;
}
/**
* MAVLink Protocol main function.
*/
int mavlink_thread_main(int argc, char *argv[])
{
int ch;
char *device_name = "/dev/ttyS1";
baudrate = 57600;
struct vehicle_status_s v_status;
struct actuator_armed_s armed;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
switch (ch) {
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if (baudrate == 0)
errx(1, "invalid baud rate '%s'", optarg);
break;
case 'd':
device_name = optarg;
break;
case 'e':
mavlink_link_termination_allowed = true;
break;
case 'o':
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
break;
default:
usage();
}
}
struct termios uart_config_original;
bool usb_uart;
/* print welcome text */
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* default values for arguments */
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
err(1, "could not open %s", device_name);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
receive_thread = receive_start(uart);
thread_running = true;
/* arm counter to go off immediately */
unsigned lowspeed_counter = 10;
while (!thread_should_exit) {
/* 1 Hz */
if (lowspeed_counter == 10) {
mavlink_update_system();
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
v_status.errors_count2,
v_status.errors_count3,
v_status.errors_count4);
lowspeed_counter = 0;
}
lowspeed_counter++;
/* sleep 1000 ms */
usleep(1000000);
}
/* wait for threads to complete */
pthread_join(receive_thread, NULL);
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
thread_running = false;
exit(0);
}
/**
* MAVLink Protocol main function.
*/
int mavlink_thread_main(int argc, char *argv[])
{
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&lb, 5);
int ch;
char *device_name = "/dev/ttyS1";
baudrate = 57600;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
switch (ch) {
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if (baudrate == 0)
errx(1, "invalid baud rate '%s'", optarg);
break;
case 'd':
device_name = optarg;
break;
case 'e':
mavlink_link_termination_allowed = true;
break;
case 'o':
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
break;
default:
usage();
}
}
struct termios uart_config_original;
bool usb_uart;
/* print welcome text */
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* default values for arguments */
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
err(1, "could not open %s", device_name);
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
receive_thread = receive_start(uart);
/* start the ORB receiver */
uorb_receive_thread = uorb_receive_start();
/* initialize waypoint manager */
mavlink_wpm_init(wpm);
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 230400) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
/* 10 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
/* 10 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) {
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
/* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
} else {
/* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
/* 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
/* 0.5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
/* 0.1 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
}
thread_running = true;
/* arm counter to go off immediately */
unsigned lowspeed_counter = 10;
while (!thread_should_exit) {
/* 1 Hz */
if (lowspeed_counter == 10) {
mavlink_update_system();
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
/* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
v_status.errors_count2,
v_status.errors_count3,
v_status.errors_count4);
lowspeed_counter = 0;
}
lowspeed_counter++;
/* sleep quarter the time */
usleep(25000);
/* check if waypoint has been reached against the last positions */
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
/* sleep quarter the time */
usleep(25000);
/* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send();
/* sleep quarter the time */
usleep(25000);
if (baudrate > 57600) {
mavlink_pm_queued_send();
}
/* sleep 10 ms */
usleep(10000);
/* send one string at 10 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
}
}
/* sleep 15 ms */
usleep(15000);
}
/* wait for threads to complete */
pthread_join(receive_thread, NULL);
pthread_join(uorb_receive_thread, NULL);
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
thread_running = false;
exit(0);
}
void Rover::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
if (failsafe.triggered != 0) {
system_status = MAV_STATE_CRITICAL;
}
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
switch (control_mode) {
case MANUAL:
case LEARNING:
case STEERING:
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AUTO:
case RTL:
case GUIDED:
base_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
case INITIALISING:
system_status = MAV_STATE_CALIBRATING;
break;
case HOLD:
system_status = 0;
break;
}
#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING==ENABLED)
if (control_mode != INITIALISING) {
// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
#endif
#if HIL_MODE != HIL_MODE_DISABLED
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
#endif
// we are armed if we are not initialising
if (control_mode != INITIALISING && hal.util->get_soft_armed()) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_GROUND_ROVER,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
}
/**
* @brief Send System State
*/
void communication_system_state_send(void)
{
/* send heartbeat to announce presence of this system */
mavlink_msg_heartbeat_send(MAVLINK_COMM_0, global_data.param[PARAM_SYSTEM_TYPE], global_data.param[PARAM_AUTOPILOT_TYPE], 0, 0, 0);
mavlink_msg_heartbeat_send(MAVLINK_COMM_2, global_data.param[PARAM_SYSTEM_TYPE], global_data.param[PARAM_AUTOPILOT_TYPE], 0, 0, 0);
}
/**
* Send a heartbeat
*/
static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mav_state = MAV_STATE_CALIBRATING;
uint8_t mav_mode = 0;
#ifdef AP
uint8_t mav_type = MAV_TYPE_FIXED_WING;
switch (pprz_mode) {
case PPRZ_MODE_MANUAL:
mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case PPRZ_MODE_AUTO1:
mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case PPRZ_MODE_AUTO2:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case PPRZ_MODE_HOME:
mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
break;
default:
break;
}
#else
uint8_t mav_type = MAV_TYPE_QUADROTOR;
switch (autopilot_mode) {
case AP_MODE_HOME:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_RC_DIRECT:
mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_ATTITUDE_RC_CLIMB:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
case AP_MODE_CARE_FREE_DIRECT:
mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AP_MODE_NAV:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case AP_MODE_GUIDED:
mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
default:
break;
}
#endif
if (stateIsAttitudeValid()) {
if (kill_throttle) {
mav_state = MAV_STATE_STANDBY;
} else {
mav_state = MAV_STATE_ACTIVE;
mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
}
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
mav_type,
MAV_AUTOPILOT_PPZ,
mav_mode,
0, // custom_mode
mav_state);
MAVLinkSendMessage();
}
/**
* 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;
}
}
}
}
