void receive() {
mavlink_message_t msg;
while(get_board()->get_serial()->available())
{
uint8_t c = get_board()->get_serial()->read();
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&m_status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//std::cout << "receiving messages" << std::endl;
mavlink_hil_controls_t hil_controls;
mavlink_msg_hil_controls_decode(&msg,&hil_controls);
std::cout << "roll: " << hil_controls.roll_ailerons << std::endl;
std::cout << "pitch: " << hil_controls.pitch_elevator << std::endl;
std::cout << "yaw: " << hil_controls.yaw_rudder << std::endl;
std::cout << "throttle: " << hil_controls.throttle << std::endl;
std::cout << "mode: " << hil_controls.mode << std::endl;
std::cout << "nav mode: " << hil_controls.nav_mode << std::endl;
break;
}
default:
{
std::cout << "received message: " << uint32_t(msg.msgid) << std::endl;
}
}
}
}
}
void receive(double * y) {
// receive messages
mavlink_message_t msg;
while(_comm->available())
{
uint8_t c = 0;
if (!_comm->read((char*)&c,1)) return;
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&_status))
{
switch(msg.msgid)
{
// this packet seems to me more constrictive so I
// recommend using rc channels scaled instead
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//std::cout << "receiving hil controls packet" << std::endl;
mavlink_hil_controls_t packet;
mavlink_msg_hil_controls_decode(&msg,&packet);
y[0] = packet.roll_ailerons;
y[1] = packet.pitch_elevator;
y[2] = packet.yaw_rudder;
y[3] = packet.throttle;
y[4] = packet.mode;
y[5] = packet.nav_mode;
y[6] = 0;
y[7] = 0;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
//std::cout << "receiving rc channels scaled packet" << std::endl;
mavlink_rc_channels_scaled_t packet;
mavlink_msg_rc_channels_scaled_decode(&msg,&packet);
y[0] = packet.chan1_scaled/1.0e4;
y[1] = packet.chan2_scaled/1.0e4;
y[2] = packet.chan3_scaled/1.0e4;
y[3] = packet.chan4_scaled/1.0e4;
y[4] = packet.chan5_scaled/1.0e4;
y[5] = packet.chan6_scaled/1.0e4;
y[6] = packet.chan7_scaled/1.0e4;
y[7] = packet.chan8_scaled/1.0e4;
break;
}
default:
{
//std::cout << "received message: " << uint32_t(msg.msgid) << std::endl;
}
}
}
}
}
void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
{
switch(msg->msgid) {
case MAVLINK_MSG_ID_HIL_CONTROLS:
mavlink_hil_controls_t controls;
mavlink_msg_hil_controls_decode(msg, &controls);
inputs.control[0] =(double)controls.roll_ailerons;
inputs.control[1] =(double)controls.pitch_elevator;
inputs.control[2] =(double)controls.yaw_rudder;
inputs.control[3] =(double)controls.throttle;
inputs.control[4] =(double)controls.aux1;
inputs.control[5] =(double)controls.aux2;
inputs.control[6] =(double)controls.aux3;
inputs.control[7] =(double)controls.aux4;
// publish message
double scaling = 340;
double offset = 500;
// simple check to see if we are simulating fw or mc
// we really need to get away from this HIL message
//bool is_fixed_wing = inputs.control[0] < 10.0f;
// TODO XXX: this makes SITL work again
bool is_fixed_wing = false;
input_reference_.resize(_rotor_count);
// at the moment vtol and multirotor are handled togehter
// pure fixed wing is handled differently
if (!is_fixed_wing) {
for (int i = 0; i < _rotor_count; i++) {
input_reference_[i] = inputs.control[i] * scaling + offset;
}
if (right_elevon_joint_ != NULL && left_elevon_joint_!= 0 && elevator_joint_ != 0) {
// set angles of control surface joints (this should go into a message for the correct plugin)
double roll = 0.5 * (inputs.control[4] + inputs.control[5]);
double pitch = 0.5 * (inputs.control[4] - inputs.control[5]);
left_elevon_joint_->SetAngle(0, roll);
right_elevon_joint_->SetAngle(0, -roll);
elevator_joint_->SetAngle(0, -pitch);
}
} else {
left_elevon_joint_->SetAngle(0, inputs.control[0]);
right_elevon_joint_->SetAngle(0, -inputs.control[0]);
elevator_joint_->SetAngle(0, inputs.control[1]);
propeller_joint_->SetForce(0, 2000.0f * inputs.control[3]);
}
received_first_referenc_ = true;
break;
}
}
void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
{
switch(msg->msgid) {
case MAVLINK_MSG_ID_HIL_CONTROLS:
mavlink_hil_controls_t controls;
mavlink_msg_hil_controls_decode(msg, &controls);
inputs.control[0] =(double)controls.roll_ailerons;
inputs.control[1] =(double)controls.pitch_elevator;
inputs.control[2] =(double)controls.yaw_rudder;
inputs.control[3] =(double)controls.throttle;
inputs.control[4] =(double)controls.aux1;
inputs.control[5] =(double)controls.aux2;
inputs.control[6] =(double)controls.aux3;
inputs.control[7] =(double)controls.aux4;
// publish message
double scaling = 340;
double offset = 500;
// simple check to see if we are simulating fw or mc
// we really need to get away from this HIL message
//bool is_fixed_wing = inputs.control[0] < 10.0f;
// TODO XXX: this makes SITL work again
bool is_fixed_wing = false;
input_reference_.resize(_rotor_count);
// set rotor speeds for all systems
for (int i = 0; i < _rotor_count; i++) {
input_reference_[i] = inputs.control[i] * scaling + offset;
}
// 5th rotor: pusher/puller throttle for the standard vtol plane
// XXX this won't work with hexacopters and alike
input_reference_[4] = (inputs.control[6] + 1.0f) / 2 * 1800 + (inputs.control[6] > -1 ? 500 : 0);
if (right_elevon_joint_ != NULL && left_elevon_joint_!= 0 && elevator_joint_ != 0) {
// set angles of control surface joints (this should go into a message for the correct plugin)
double roll = 0.5 * (inputs.control[4] + inputs.control[5]);
double pitch = 0.5 * (inputs.control[4] - inputs.control[5]);
left_elevon_joint_->SetAngle(0, roll);
right_elevon_joint_->SetAngle(0, -roll);
elevator_joint_->SetAngle(0, -pitch);
}
received_first_referenc_ = true;
break;
}
}
void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
{
switch(msg->msgid) {
case MAVLINK_MSG_ID_HIL_CONTROLS:
mavlink_hil_controls_t controls;
mavlink_msg_hil_controls_decode(msg, &controls);
inputs.control[0] =(double)controls.roll_ailerons;
inputs.control[1] =(double)controls.pitch_elevator;
inputs.control[2] =(double)controls.yaw_rudder;
inputs.control[3] =(double)controls.throttle;
inputs.control[4] =(double)controls.aux1;
inputs.control[5] =(double)controls.aux2;
inputs.control[6] =(double)controls.aux3;
inputs.control[7] =(double)controls.aux4;
// publish message
double scaling = 340;
double offset = 500;
input_reference_.resize(_rotor_count);
for (int i = 0; i < _rotor_count; i++) {
input_reference_[i] = inputs.control[i] * scaling + offset;
}
if (right_elevon_joint_ != NULL && left_elevon_joint_!= 0 && elevator_joint_ != 0) {
// set angles of control surface joints (this should go into a message for the correct plugin)
double roll = 0.5 * (inputs.control[4] + inputs.control[5]);
double pitch = 0.5 * (inputs.control[4] - inputs.control[5]);
left_elevon_joint_->SetAngle(0, roll);
right_elevon_joint_->SetAngle(0, -roll);
elevator_joint_->SetAngle(0, -pitch);
}
received_first_referenc_ = true;
break;
}
}
void GazeboMavlinkInterface::MavlinkControlCallback(const mavros::Mavlink::ConstPtr &rmsg) {
mavlink_message_t mmsg;
if(mavutils::copy_ros_to_mavlink(rmsg, mmsg)){
mavlink_hil_controls_t act_msg;
mavlink_message_t* msg = &mmsg;
mavlink_msg_hil_controls_decode(msg, &act_msg);
inputs.control[0] =(double)act_msg.roll_ailerons;
inputs.control[1] =(double)act_msg.pitch_elevator;
inputs.control[2] =(double)act_msg.yaw_rudder;
inputs.control[3] =(double)act_msg.throttle;
inputs.control[4] =(double)act_msg.aux1;
inputs.control[5] =(double)act_msg.aux2;
inputs.control[6] =(double)act_msg.aux3;
inputs.control[7] =(double)act_msg.aux4;
// publish message
double scaling = 150;
double offset = 600;
mav_msgs::CommandMotorSpeedPtr turning_velocities_msg(new mav_msgs::CommandMotorSpeed);
for (int i = 0; i < _rotor_count; i++) {
turning_velocities_msg->motor_speed.push_back(inputs.control[i] * scaling + offset);
}
CommandMotorMavros(turning_velocities_msg);
turning_velocities_msg.reset();
} else{
std::cout << "incorrect mavlink data" <<"\n";
}
}
void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
{
switch(msg->msgid) {
case MAVLINK_MSG_ID_HIL_CONTROLS:
mavlink_hil_controls_t controls;
mavlink_msg_hil_controls_decode(msg, &controls);
bool armed = false;
if ((controls.mode & MAV_MODE_FLAG_SAFETY_ARMED) > 0) {
armed = true;
}
const unsigned n_out = sizeof(inputs.control) / sizeof(inputs.control[0]);
const unsigned block_size = 8;
unsigned off = (controls.nav_mode * block_size);
// We only support 8 outputs so far, so we
// ignore the second, third and fourth output
// XXX setting this to anything higher than 8 results
// in memory smashing, likely due to a hardcoded
// motor count somewhere
if (off + block_size > 8) {
break;
}
inputs.control[off + 0] = controls.roll_ailerons;
inputs.control[off + 1] = controls.pitch_elevator;
inputs.control[off + 2] = controls.yaw_rudder;
inputs.control[off + 3] = controls.throttle;
inputs.control[off + 4] = controls.aux1;
inputs.control[off + 5] = controls.aux2;
inputs.control[off + 6] = controls.aux3;
inputs.control[off + 7] = controls.aux4;
bool is_vtol = (right_elevon_joint_ != nullptr);
// Set all scalings
// Initialize all outputs as motors
// if joints are present these will be
// reconfigured in the next step
for (unsigned i = off; i < off + block_size; i++) {
input_index[i] = i;
// scaling values
input_offset[i] = 1.0;
// XXX this needs re-investigation regarding
// the correct scaling for the correct motor
// model
input_scaling[i] = 550.0;
zero_position_disarmed[i] = 0.0;
zero_position_armed[i] = (is_vtol) ? 0.0 : 100.0;
}
if (is_vtol) {
// Config for standard VTOL model
// Fift motor
input_index[off + 4] = off + 4;
input_offset[off + 4] = 1.0;
input_scaling[off + 4] = 1600;
zero_position_disarmed[off + 4] = 0.0;
zero_position_armed[off + 4] = 0.0;
// Servos
for (unsigned i = off + 5; i < off + block_size; i++) {
// scaling values
input_index[i] = i;
input_offset[i] = 0.0;
input_scaling[i] = 1.0;
zero_position_disarmed[i] = 0.0;
zero_position_armed[i] = 0.0;
}
}
last_actuator_time_ = world_->GetSimTime();
input_reference_.resize(n_out);
// set rotor speeds
for (int i = 0; i < input_reference_.size(); i++) {
if (armed) {
input_reference_[i] = (inputs.control[input_index[i]] + input_offset[i]) * input_scaling[i] + zero_position_armed[i];
} else {
input_reference_[i] = zero_position_disarmed[i];
}
}
// set joint positions
for (int i = 0; i < input_reference_.size(); i++) {
if (joints_[i]) {
#if GAZEBO_MAJOR_VERSION >= 6
joints_[i]->SetPosition(0, input_reference_[i]);
#else
joints_[i]->SetAngle(0, input_reference_[i]);
#endif
}
}
// legacy method, can eventually be replaced
if (right_elevon_joint_ != NULL && left_elevon_joint_!= 0 && elevator_joint_ != 0) {
#if GAZEBO_MAJOR_VERSION >= 6
left_elevon_joint_->SetPosition(0, input_reference_[5]);
right_elevon_joint_->SetPosition(0, input_reference_[6]);
elevator_joint_->SetPosition(0, input_reference_[7]);
#else
left_elevon_joint_->SetAngle(0, input_reference_[5]);
right_elevon_joint_->SetAngle(0, input_reference_[6]);
elevator_joint_->SetAngle(0, input_reference_[7]);
#endif
}
received_first_referenc_ = true;
break;
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
int success = 0; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
int err_counter = 0;
struct timespec diff_r;
struct timespec aft_read;
struct timespec bef_read;
struct timespec receive_time;
struct timespec longRelNanoSleep;
longRelNanoSleep.tv_sec = 0;
longRelNanoSleep.tv_nsec = 100000;
struct timespec RelNanoSleep;
RelNanoSleep.tv_sec = 0;
RelNanoSleep.tv_nsec = 10;
// Blocking wait for new data
// pthread_mutex_lock(&mut);
// if (blocked == 0 && writing_status)
// {
// blocked = 1;
// pthread_cond_wait(&cond,&mut);
// blocked = 0;
// }
// else
// {
// pthread_cond_signal(&cond);
// }
// clock_gettime(CLOCK_REALTIME,&bef_read);
// clock_gettime(CLOCK_REALTIME,&aft_read);
// timespec_sub(&diff_r,&aft_read,&bef_read);
// int64_t diff_r_us = diff_r.tv_sec*1e6 + diff_r.tv_nsec/1e3;
// printf("Read Time: %ld \n",diff_r_us);
//
// pthread_mutex_unlock(&mut);
mavlink_message_t message;
while ( (success != 1) && (!time_to_exit) )
{
// ----------------------------------------------------------------------
// READ ONE BYTE AT TIME UNTIL COMPLETE MESSAGE IS RECEIVED
// ----------------------------------------------------------------------
success = serial_port->read_message(message); //Have I obtained a complete message?
if ( success == -1) // Errors...retry...
{
err_counter++;
if (err_counter == 3) // To many void read
{
fprintf(stderr,"ERRORS!\n");
break;
}
}
}
err_counter = 0;
if (success == -1) return;
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
//if( success )
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
base_mode = current_messages.heartbeat.base_mode;
mav_type = current_messages.heartbeat.type;
system_status = current_messages.heartbeat.system_status;
custom_mode = current_messages.heartbeat.custom_mode;
//printf("base_mode = %u\n",base_mode);
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
heartbeat_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.heartbeat - read_heartbeat_old) > 10000000 )
{
printf("HEARTBEAT frequency : %d \n",heartbeat_count/10);
heartbeat_count = 0;
read_heartbeat_old = current_messages.time_stamps.heartbeat;
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
clock_gettime(CLOCK_REALTIME,&receive_time);
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
highres_imu_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.highres_imu - read_highres_imu_old) > 10000000 )
{
printf("HIGHRES_IMU frequency : %d Hz\n",highres_imu_count/10);
highres_imu_count = 0;
read_highres_imu_old = current_messages.time_stamps.highres_imu;
}
//fprintf(logfile_rd,"HIGHRES_IMU %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//printf("MAVLINK_MSG_ID_HIL_CONTROLS\n");
mavlink_msg_hil_controls_decode(&message, &(current_messages.hil_controls));
current_messages.time_stamps.hil_controls = get_time_usec();
this_timestamps.hil_controls = current_messages.time_stamps.hil_controls;
float data[4] = {current_messages.hil_controls.roll_ailerons,
current_messages.hil_controls.pitch_elevator,
current_messages.hil_controls.yaw_rudder,
current_messages.hil_controls.throttle};
int bsent = udp_mtl->send_bytes((char *)data,sizeof(float[4]));
hil_controls_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.hil_controls - read_hil_controls_old) > 10000000 )
{
printf("HIL_CONTROLS frequency : %d Hz\n",hil_controls_count/10);
hil_controls_count = 0;
read_hil_controls_old = current_messages.time_stamps.hil_controls;
printf("%0.2f | ",current_messages.hil_controls.roll_ailerons);
printf("%0.2f | ",current_messages.hil_controls.pitch_elevator);
printf("%0.2f | ",current_messages.hil_controls.yaw_rudder);
printf("%0.2f | \n",current_messages.hil_controls.throttle);
}
//fprintf(logfile_rd,"HIL_CONTROLS %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
return;
}
