void handle_raw_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (has_hr_imu || has_scaled_imu)
return;
sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
mavlink_raw_imu_t imu_raw;
mavlink_msg_raw_imu_decode(msg, &imu_raw);
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id;
/* NOTE: APM send SCALED_IMU data as RAW_IMU */
fill_imu_msg_raw(imu_msg,
imu_raw.xgyro * MILLIRS_TO_RADSEC,
-imu_raw.ygyro * MILLIRS_TO_RADSEC,
-imu_raw.zgyro * MILLIRS_TO_RADSEC,
imu_raw.xacc * MILLIG_TO_MS2,
-imu_raw.yacc * MILLIG_TO_MS2,
-imu_raw.zacc * MILLIG_TO_MS2);
if (!uas->is_ardupilotmega()) {
ROS_WARN_THROTTLE_NAMED(60, "imu", "RAW_IMU: linear acceleration known on APM only");
linear_accel_vec.x = 0.0;
linear_accel_vec.y = 0.0;
linear_accel_vec.z = 0.0;
}
imu_msg->header = header;
imu_raw_pub.publish(imu_msg);
/* -*- magnetic vector -*- */
sensor_msgs::MagneticFieldPtr magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
// Convert from local NED plane to ENU
magn_msg->magnetic_field.x = imu_raw.ymag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.y = imu_raw.xmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.z = -imu_raw.zmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field_covariance = magnetic_cov;
magn_msg->header = header;
magn_pub.publish(magn_msg);
}
void ofxMavlink::threadedFunction() {
while( isThreadRunning() != 0 ) {
// static unsigned int imu_receive_counter =
uint8_t cp;
mavlink_message_t message;
mavlink_status_t status;
uint8_t msgReceived = false;
if (read(fd, &cp, 1) > 0)
{
// Check if a message could be decoded, return the message in case yes
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
if (status.packet_rx_drop_count > 0)
{
if(bDebug) ofLogWarning("ofxMavlink") << "ERROR: DROPPED " << status.packet_rx_drop_count <<" PACKETS:" << " " << cp;
}
}
else
{
if(bDebug) ofLogError("ofxMavlink") << "ERROR: Could not read from fd " << fd;
}
// If a message could be decoded, handle it
if(msgReceived)
{
msgcount++;
if (bDebug)
{
fprintf(stderr,"Received serial data: ");
unsigned int i;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message);
if (messageLength > MAVLINK_MAX_PACKET_LEN)
{
fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
}
else
{
for (i=0; i<messageLength; i++)
{
unsigned char v=buffer[i];
fprintf(stderr,"%02x ", v);
}
fprintf(stderr,"\n");
}
}
//if(bDebug) ofLogNotice("ofxMavlink") << "Received message from serial with ID #" << message.msgid << " (sys:" << message.sysid << "|comp:" << message.compid << "):\n";
switch (message.msgid)
{
case MAVLINK_MSG_ID_COMMAND_ACK:
{
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &ack);
switch (ack.result)
{
case MAV_RESULT_ACCEPTED:
{
ofLogNotice("ofxMavlink") << "SUCCESS: Executed Command: " << ack.command;
}
break;
case MAV_RESULT_TEMPORARILY_REJECTED:
{
ofLogError("ofxMavlink") << "FAILURE: Temporarily rejected Command: " << ack.command;
}
break;
case MAV_RESULT_DENIED:
{
ofLogError("ofxMavlink") << "FAILURE: Denied Command: " << ack.command;
}
break;
case MAV_RESULT_UNSUPPORTED:
{
ofLogError("ofxMavlink") << "FAILURE: Unsupported Command: " << ack.command;
}
break;
case MAV_RESULT_FAILED:
{
ofLogError("ofxMavlink") << "FAILURE: Failed Command: " << ack.command;
}
break;
}
break;
}
// case MAVLINK_MSG_ID_COMMAND_ACK:
// {
// mavlink_command_ack_t ack;
// mavlink_msg_command_ack_decode(&message,&ack);
// cout << "received CMD_ACK command=" << ack.command << " result=" << ack.result << endl;
//
// if(ack.command == MAV_CMD_COMPONENT_ARM_DISARM) {
// cout << "Acknowledge arm/disarm command";
//
// }
// if(ack.result == MAV_CMD_ACK_OK) {
// cout << "received CMD_ACK_OK" << endl;
// }
// break;
// }
case MAVLINK_MSG_ID_RAW_IMU:
{
mavlink_raw_imu_t imu;
mavlink_msg_raw_imu_decode(&message, &imu);
lock();
time_usec = imu.time_usec;
unlock();
ofLogVerbose("ofxMavlink") << "-- RAW_IMU message received --" << endl;
ofLogVerbose("ofxMavlink") << "\t time: (us) " << imu.time_usec << endl;
ofLogVerbose("ofxMavlink") << "\t acc:" << imu.xacc << " " << imu.yacc << " " << imu.zacc << endl;
ofLogVerbose("ofxMavlink") << "\t gyro: (rad/s)" << imu.xgyro << " " << imu.ygyro << " " << imu.zgyro << endl;
ofLogVerbose("ofxMavlink") << "\t mag: (Ga)" << imu.xmag << " " << imu.ymag << " " << imu.zmag << endl;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
ofLogNotice("ofxMavlink") << "-- GPS RAW INT message received --" << endl;
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(&message, &packet);
if (packet.fix_type > 2)
{
lock();
latitude = packet.lat/(double)1E7;
longitude = packet.lon/(double)1E7;
altitude = packet.alt/1000.0;
unlock();
ofLogNotice("ofxMavlink") << "Altitude:" << packet.alt/1000.0 << "latitude:" << (float)packet.lat / 10000000.0 << " longitude:" << (float)packet.lon / 10000000.0 << " ";
ofLogNotice("ofxMavlink") << "Satellites visible:" << packet.satellites_visible << " GPS fix:" << packet.fix_type << endl;
// if (packet.lat != 0.0) {
//
// latitude = (float)packet.lat;
// longitude = (float)packet.lon;
// altitude = (float)packet.alt;
// //ModelData.speed = (float)packet.vel / 100.0;
// //ModelData.numSat = packet.satellites_visible;
// gpsfix = packet.fix_type;
}
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(&message, &packet);
int droneType = packet.type;
int autoPilot = packet.autopilot;
// cout << "base mode:" << packet.base_mode << " custom mode:" << packet.custom_mode << endl;
// if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
// //ModelData.mode = MODEL_MODE_MANUAL;
// cout << "Manual Mode" << endl;
// } else if (packet.base_mode == 128 + 64 + 16) {
// //ModelData.mode = MODEL_MODE_RTL;
// cout << "RTL Mode" << endl;
// } else if (packet.base_mode == 128 + 16) {
// //ModelData.mode = MODEL_MODE_POSHOLD;
// cout << "Poshold Mode" << endl;
// } else if (packet.base_mode == 128 + 4) {
// //ModelData.mode = MODEL_MODE_MISSION;
// cout << "Mission Mode" << endl;
// }
// if(packet.base_mode == MAV_MODE_STABILIZE_DISARMED)
// {
// cout << "Stablilize disarmed mode" << endl;
// }
ofLogNotice("ofxMavlink") << "-- Heartbeat -- sysId:" << message.sysid << " compId:" << message.compid << " drone type:" << droneType << " autoPilot:" << autoPilot;
// if(droneType == MAV_TYPE_QUADROTOR) cout << " Quadrotor"; else cout << droneType;
// cout << " Autopilot:";
// if(autoPilot == MAV_AUTOPILOT_ARDUPILOTMEGA) cout << " ArduPilotMega"; else cout << autoPilot;
// cout << endl;
if (message.sysid != 0xff) {
lock();
targetId = message.sysid;
compId = message.compid;
//cout << "compid = " << message.compid << " sysid =" << message.sysid;
unlock();
}
break;
}
default:
break;
}
}
}
}
static void
handle_message(mavlink_message_t *msg)
{
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */
printf("[mavlink] Terminating .. \n");
fflush(stdout);
usleep(50000);
/* terminate other threads and this thread */
thread_should_exit = true;
} else {
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
vcmd.command = cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
}
/* publish */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
}
if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(msg, &flow);
struct optical_flow_s f;
f.timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
/* check if topic is advertised */
if (flow_pub <= 0) {
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
/* publish */
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
}
}
if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
/* Set mode on request */
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = new_mode.custom_mode;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = MAV_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
/* Handle Vicon position estimates */
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
vicon_position.timestamp = hrt_absolute_time();
vicon_position.x = pos.x;
vicon_position.y = pos.y;
vicon_position.z = pos.z;
vicon_position.roll = pos.roll;
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (vicon_position_pub <= 0) {
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
}
}
/* Handle quadrotor motor setpoints */
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
if (mavlink_system.sysid < 4) {
/* switch to a receiving link mode */
gcs_link = false;
/*
* rate control mode - defined by MAVLink
*/
uint8_t ml_mode = 0;
bool ml_armed = false;
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
ml_armed = true;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
ml_armed = true;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
case 4:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
break;
}
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
ml_armed = false;
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = ml_mode;
offboard_control_sp.timestamp = hrt_absolute_time();
/* check if topic has to be advertised */
if (offboard_control_sp_pub <= 0) {
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
/* Publish */
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
}
}
}
/*
* Only decode hil messages in HIL mode.
*
* The HIL mode is enabled by the HIL bit flag
* in the system mode. Either send a set mode
* COMMAND_LONG message or a SET_MODE message
*/
if (mavlink_hil_enabled) {
uint64_t timestamp = hrt_absolute_time();
/* TODO, set ground_press/ temp during calib */
static const float ground_press = 1013.25f; // mbar
static const float ground_tempC = 21.0f;
static const float ground_alt = 0.0f;
static const float T0 = 273.15;
static const float R = 287.05f;
static const float g = 9.806f;
if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
mavlink_raw_imu_t imu;
mavlink_msg_raw_imu_decode(msg, &imu);
/* packet counter */
static uint16_t hil_counter = 0;
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = imu.time_usec;
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro;
hil_sensors.gyro_raw[1] = imu.ygyro;
hil_sensors.gyro_raw[2] = imu.zgyro;
hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc;
hil_sensors.accelerometer_raw[1] = imu.yacc;
hil_sensors.accelerometer_raw[2] = imu.zacc;
hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
/* adc */
hil_sensors.adc_voltage_v[0] = 0;
hil_sensors.adc_voltage_v[1] = 0;
hil_sensors.adc_voltage_v[2] = 0;
/* magnetometer */
float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.magnetometer_raw[0] = imu.xmag;
hil_sensors.magnetometer_raw[1] = imu.ymag;
hil_sensors.magnetometer_raw[2] = imu.zmag;
hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
/* publish */
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
// increment counters
hil_counter += 1 ;
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil imu at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
}
if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
mavlink_highres_imu_t imu;
mavlink_msg_highres_imu_decode(msg, &imu);
/* packet counter */
static uint16_t hil_counter = 0;
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = imu.time_usec;
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
/* adc */
hil_sensors.adc_voltage_v[0] = 0;
hil_sensors.adc_voltage_v[1] = 0;
hil_sensors.adc_voltage_v[2] = 0;
/* magnetometer */
float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
hil_sensors.magnetometer_ga[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag;
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
hil_sensors.baro_pres_mbar = imu.abs_pressure;
float tempC = imu.temperature;
float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure);
hil_sensors.baro_alt_meter = h;
hil_sensors.baro_temp_celcius = imu.temperature;
/* publish */
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
// increment counters
hil_counter += 1 ;
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil imu at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
}
if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
mavlink_gps_raw_int_t gps;
mavlink_msg_gps_raw_int_decode(msg, &gps);
/* packet counter */
static uint16_t hil_counter = 0;
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* gps */
hil_gps.timestamp_position = gps.time_usec;
// hil_gps.counter = hil_counter++;
hil_gps.time_gps_usec = gps.time_usec;
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
// hil_gps.counter_pos_valid = hil_counter++;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance?
hil_gps.p_variance_m = 100; // XXX 100 m variance?
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
hil_gps.vel_d_m_s = 0.0f;
hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
/* publish */
orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
// increment counters
hil_counter += 1 ;
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil gps at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
}
if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
mavlink_raw_pressure_t press;
mavlink_msg_raw_pressure_decode(msg, &press);
/* packet counter */
static uint16_t hil_counter = 0;
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = press.time_usec;
/* baro */
float tempC = press.temperature / 100.0f;
float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
hil_sensors.baro_counter = hil_counter;
hil_sensors.baro_pres_mbar = press.press_abs;
hil_sensors.baro_alt_meter = h;
hil_sensors.baro_temp_celcius = tempC;
/* publish */
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
// increment counters
hil_counter += 1 ;
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil pressure at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
}
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
mavlink_hil_state_t hil_state;
mavlink_msg_hil_state_decode(msg, &hil_state);
/* Calculate Rotation Matrix */
//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
//TODO: assuming low pitch and roll values for now
hil_attitude.R[0][0] = cosf(hil_state.yaw);
hil_attitude.R[0][1] = sinf(hil_state.yaw);
hil_attitude.R[0][2] = 0.0f;
hil_attitude.R[1][0] = -sinf(hil_state.yaw);
hil_attitude.R[1][1] = cosf(hil_state.yaw);
hil_attitude.R[1][2] = 0.0f;
hil_attitude.R[2][0] = 0.0f;
hil_attitude.R[2][1] = 0.0f;
hil_attitude.R[2][2] = 1.0f;
hil_attitude.R_valid = true;
}
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
/* set timestamp and notify processes (broadcast) */
hil_global_pos.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
hil_attitude.roll = hil_state.roll;
hil_attitude.pitch = hil_state.pitch;
hil_attitude.yaw = hil_state.yaw;
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
/* set timestamp and notify processes (broadcast) */
hil_attitude.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
struct rc_channels_s rc_hil;
memset(&rc_hil, 0, sizeof(rc_hil));
static orb_advert_t rc_pub = 0;
rc_hil.timestamp = hrt_absolute_time();
rc_hil.chan_count = 4;
rc_hil.chan[0].scaled = man.x / 1000.0f;
rc_hil.chan[1].scaled = man.y / 1000.0f;
rc_hil.chan[2].scaled = man.r / 1000.0f;
rc_hil.chan[3].scaled = man.z / 1000.0f;
struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0;
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
mc.timestamp = rc_hil.timestamp;
mc.roll = man.x / 1000.0f;
mc.pitch = man.y / 1000.0f;
mc.yaw = man.r / 1000.0f;
mc.throttle = man.z / 1000.0f;
/* fake RC channels with manual control input from simulator */
if (rc_pub == 0) {
rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
} else {
orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
}
if (mc_pub == 0) {
mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
} else {
orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
}
}
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// 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:
{
std::cout << "MAVLINK_MSG_ID_HEARTBEAT" << std::endl;
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//std::cout << "MAVLINK_MSG_ID_SYS_STATUS" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_BATTERY_STATUS" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_RADIO_STATUS" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_LOCAL_POSITION_NED" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_GLOBAL_POSITION_INT" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_HIGHRES_IMU" << std::endl;
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;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_ATTITUDE" << std::endl;
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
quaternion_from_euler(&quaternion, current_messages.attitude.roll,
current_messages.attitude.pitch, current_messages.attitude.yaw);
std::cout <<
"\troll: " << current_messages.attitude.roll <<
"\tpitch: " << current_messages.attitude.pitch <<
"\tyaw: " << current_messages.attitude.yaw <<
std::endl;
std::cout <<
"\trollspeed: " << current_messages.attitude.rollspeed <<
"\tpitchspeed: " << current_messages.attitude.pitchspeed <<
"\tyawspeed: " << current_messages.attitude.yawspeed <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
break;
}
case MAVLINK_MSG_ID_DEBUG:
{
std::cout << "MAVLINK_MSG_ID_DEBUG" << std::endl;
/* mavlink_msg_debug_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_STATUSTEXT:
{
std::cout << "MAVLINK_MSG_ID_STATUSTEXT: ";
mavlink_msg_statustext_decode(&message, &(current_messages.statustext));
current_messages.statustext.text[50] = 0;
printf("%d - '%s'\n", current_messages.statustext.severity, current_messages.statustext.text);
current_messages.time_stamps.statustext = get_time_usec();
this_timestamps.statustext = current_messages.time_stamps.statustext;
break;
}
case MAVLINK_MSG_ID_RAW_IMU:
{
std::cout << "MAVLINK_MSG_ID_RAW_IMU:" << std::endl;
mavlink_msg_raw_imu_decode(&message, &(current_messages.raw_imu));
std::cout << "\tacc :\t" << current_messages.raw_imu.xacc <<
"\t" << current_messages.raw_imu.yacc <<
"\t" << current_messages.raw_imu.zacc <<
std::endl;
std::cout << "\tgyro:\t" << current_messages.raw_imu.xgyro <<
"\t" << current_messages.raw_imu.ygyro <<
"\t" << current_messages.raw_imu.zgyro <<
std::endl;
std::cout << "\tmag:\t" << current_messages.raw_imu.xmag <<
"\t" << current_messages.raw_imu.ymag <<
"\t" << current_messages.raw_imu.zmag <<
std::endl;
current_messages.time_stamps.raw_imu = get_time_usec();
this_timestamps.raw_imu = current_messages.time_stamps.raw_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
std::cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << std::endl;
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
current_messages.time_stamps.gps_raw_int = get_time_usec();
this_timestamps.gps_raw_int = current_messages.time_stamps.gps_raw_int;
std::cout <<
"\tlat: " << current_messages.gps_raw_int.lat <<
"\tlon: " << current_messages.gps_raw_int.lon <<
"\talt: " << current_messages.gps_raw_int.alt <<
std::endl;
std::cout <<
"\teph: " << current_messages.gps_raw_int.eph <<
"\tepv: " << current_messages.gps_raw_int.epv <<
std::endl;
std::cout <<
"\tvel: " << current_messages.gps_raw_int.vel <<
"\tcog: " << current_messages.gps_raw_int.cog <<
std::endl;
std::cout <<
"\tfix: " << (int)current_messages.gps_raw_int.fix_type <<
"\tsat: " << (int)current_messages.gps_raw_int.satellites_visible <<
std::endl;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:" << std::endl;
mavlink_msg_nav_controller_output_decode(&message, &(current_messages.nav_controller_output));
current_messages.time_stamps.nav_controller_output = get_time_usec();
this_timestamps.nav_controller_output = current_messages.time_stamps.nav_controller_output;
quaternion_from_euler(&quaternion, current_messages.nav_controller_output.nav_roll,
current_messages.nav_controller_output.nav_pitch, current_messages.nav_controller_output.nav_bearing);
std::cout <<
"\tnav_roll: " << current_messages.nav_controller_output.nav_roll <<
"\tnav_pitch: " << current_messages.nav_controller_output.nav_pitch <<
"\tnav_bearing: " << current_messages.nav_controller_output.nav_bearing <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
std::cout <<
"\ttarget_bearing: " << current_messages.nav_controller_output.target_bearing <<
"\twp_dist: " << current_messages.nav_controller_output.wp_dist <<
std::endl;
std::cout <<
"\talt_error: " << current_messages.nav_controller_output.alt_error <<
"\taspd_error: " << current_messages.nav_controller_output.aspd_error <<
"\txtrack_error: " << current_messages.nav_controller_output.xtrack_error <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
std::cout << "MAVLINK_MSG_ID_SCALED_PRESSURE:" << std::endl;
mavlink_msg_scaled_pressure_decode(&message, &(current_messages.scaled_pressure));
current_messages.time_stamps.scaled_pressure = get_time_usec();
this_timestamps.scaled_pressure = current_messages.time_stamps.scaled_pressure;
std::cout <<
"\tpress_abs: " << current_messages.scaled_pressure.press_abs <<
"\tpress_diff: " << current_messages.scaled_pressure.press_diff <<
"\ttemperature: " << ((double)current_messages.scaled_pressure.temperature / 100.0) <<
std::endl;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{
std::cout << "MAVLINK_MSG_ID_RC_CHANNELS_RAW:" << std::endl;
mavlink_msg_rc_channels_raw_decode(&message, &(current_messages.rc_channels_raw));
current_messages.time_stamps.rc_channels_raw = get_time_usec();
this_timestamps.rc_channels_raw = current_messages.time_stamps.rc_channels_raw;
std::cout <<
"\tport: " << (int)current_messages.rc_channels_raw.port <<
"\tchan1_raw: " << current_messages.rc_channels_raw.chan1_raw <<
"\tchan2_raw: " << current_messages.rc_channels_raw.chan2_raw <<
"\tchan3_raw: " << current_messages.rc_channels_raw.chan3_raw <<
"\tchan4_raw: " << current_messages.rc_channels_raw.chan4_raw <<
"\tchan5_raw: " << current_messages.rc_channels_raw.chan5_raw <<
"\tchan6_raw: " << current_messages.rc_channels_raw.chan6_raw <<
"\tchan7_raw: " << current_messages.rc_channels_raw.chan7_raw <<
"\tchan8_raw: " << current_messages.rc_channels_raw.chan8_raw <<
"\trssi: " << (int)current_messages.rc_channels_raw.rssi <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
std::cout << "MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:" << std::endl;
mavlink_msg_servo_output_raw_decode(&message, &(current_messages.servo_output_raw));
current_messages.time_stamps.servo_output_raw = get_time_usec();
this_timestamps.servo_output_raw = current_messages.time_stamps.servo_output_raw;
std::cout <<
"\tport: " << (int)current_messages.servo_output_raw.port <<
"\tservo1_raw: " << current_messages.servo_output_raw.servo1_raw <<
"\tservo2_raw: " << current_messages.servo_output_raw.servo2_raw <<
"\tservo3_raw: " << current_messages.servo_output_raw.servo3_raw <<
"\tservo4_raw: " << current_messages.servo_output_raw.servo4_raw <<
"\tservo5_raw: " << current_messages.servo_output_raw.servo5_raw <<
"\tservo6_raw: " << current_messages.servo_output_raw.servo6_raw <<
"\tservo7_raw: " << current_messages.servo_output_raw.servo7_raw <<
"\tservo8_raw: " << current_messages.servo_output_raw.servo8_raw <<
std::endl;
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
std::cout << "MAVLINK_MSG_ID_VFR_HUD:" << std::endl;
mavlink_msg_vfr_hud_decode(&message, &(current_messages.vfr_hud));
current_messages.time_stamps.vfr_hud = get_time_usec();
this_timestamps.vfr_hud = current_messages.time_stamps.vfr_hud;
std::cout <<
"\tairspeed: " << current_messages.vfr_hud.airspeed <<
"\tgroundspeed: " << current_messages.vfr_hud.groundspeed <<
"\theading: " << current_messages.vfr_hud.heading <<
std::endl;
std::cout <<
"\tthrottle: " << current_messages.vfr_hud.throttle <<
"\talt: " << current_messages.vfr_hud.alt <<
"\tclimb: " << current_messages.vfr_hud.climb <<
std::endl;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT:
{
std::cout << "MAVLINK_MSG_ID_MISSION_CURRENT: ";
mavlink_msg_mission_current_decode(&message, &(current_messages.mission_current));
current_messages.time_stamps.mission_current = get_time_usec();
this_timestamps.mission_current = current_messages.time_stamps.mission_current;
std::cout <<
"seq: " << current_messages.mission_current.seq <<
std::endl;
break;
}
default:
{
printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
this_timestamps.sys_status &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
this_timestamps.position_target_global_int &&
this_timestamps.highres_imu &&
this_timestamps.attitude ;
// give the write thread time to use the port
if ( writing_status > false )
usleep(100); // look for components of batches at 10kHz
} // end: while not received all
return;
}
void
ArduPilot::readData(){
mavlink_msgs::Mavlink ros_mav_msg;
mavlink_message_t msg;
mavlink_status_t status;
uint8_t byte;
while(my_serial_.available()>0){
my_serial_.read(&byte,1);
if(mavlink_parse_char(MAVLINK_COMM_0,byte,&msg,&status)){
clock_gettime(CLOCK_MONOTONIC, ¤t_time_);
createROSFromMavlink(&msg,&ros_mav_msg);
ros_mav_msg.header.stamp.sec = current_time_.tv_sec;
ros_mav_msg.header.stamp.nsec = current_time_.tv_nsec;
mav_pub_.publish(ros_mav_msg);
switch(msg.msgid){
case MAVLINK_MSG_ID_HEARTBEAT:
{
}
break;
case MAVLINK_MSG_ID_RAW_IMU: // msg ID: 27
{
if(imu_pub_.getNumSubscribers()>0){
mavlink_raw_imu_t imu;
mavlink_msg_raw_imu_decode(&msg,&imu);
ros_imu_msg_.header.stamp.sec = current_time_.tv_sec;
ros_imu_msg_.header.stamp.nsec = current_time_.tv_nsec;
//ros_imu_msg_.header.stamp = ros::Time.now();
ros_imu_msg_.linear_acceleration.x = 9.81*(imu.xacc/1000);
ros_imu_msg_.linear_acceleration.y = 9.81*(imu.yacc/1000);
ros_imu_msg_.linear_acceleration.z = 9.81*(imu.zacc/1000);
ros_imu_msg_.angular_velocity.x = imu.xgyro/1000;
ros_imu_msg_.angular_velocity.y = imu.ygyro/1000;
ros_imu_msg_.angular_velocity.z = imu.zgyro/1000;
imu_pub_.publish(ros_imu_msg_);
}
}
break;
case MAVLINK_MSG_ID_GPS_RAW_INT: //msg ID: 24
{
if(gps_pub_.getNumSubscribers()>0){
mavlink_gps_raw_int_t gps;
mavlink_msg_gps_raw_int_decode(&msg,&gps);
ros_gps_msg_.header.stamp.sec = current_time_.tv_sec;
ros_gps_msg_.header.stamp.nsec = current_time_.tv_nsec;
ros_gps_msg_.latitude = gps.lat/1E7;
ros_gps_msg_.longitude = gps.lon/1E7;
ros_gps_msg_.altitude = gps.alt/1E3;
gps_pub_.publish(ros_gps_msg_);
gps_common::LLtoUTM(gps.lat,gps.lon, north_, east_, utm_zone_);
ros_pose_msg_.position.x = north_;
ros_pose_msg_.position.y = east_;
ros_pose_msg_.position.z = -gps.alt/1E7;
}
}
break;
case MAVLINK_MSG_ID_ATTITUDE: //msg ID: 30
{
if(pose_pub_.getNumSubscribers()>0){
mavlink_attitude_t att;
mavlink_msg_attitude_decode(&msg,&att);
ros_att_msg_.header.stamp.sec = current_time_.tv_sec;
ros_att_msg_.header.stamp.nsec = current_time_.tv_nsec;
ros_att_msg_.orientation.x = att.roll;
ros_att_msg_.orientation.y = att.pitch;
ros_att_msg_.orientation.z = att.yaw;
att_pub_.publish(ros_att_msg_);
ros_pose_msg_.orientation.x = att.roll;
ros_pose_msg_.orientation.y = att.pitch;
ros_pose_msg_.orientation.z = att.yaw;
pose_pub_.publish(ros_pose_msg_);
}
}
break;
case MAVLINK_MSG_ID_AHRS: //msg ID: 163
{
mavlink_ahrs_t ahrs;
}
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //msg ID: 35
{
if(rc_pub_.getNumSubscribers()>0){
mavlink_rc_channels_raw_t rc;
ros_rc_msg_.time_boot_ms = rc.time_boot_ms;
ros_rc_msg_.chan1_raw = rc.chan1_raw;
ros_rc_msg_.chan2_raw = rc.chan2_raw;
ros_rc_msg_.chan3_raw = rc.chan3_raw;
ros_rc_msg_.chan4_raw = rc.chan4_raw;
ros_rc_msg_.chan5_raw = rc.chan5_raw;
ros_rc_msg_.chan6_raw = rc.chan6_raw;
ros_rc_msg_.chan7_raw = rc.chan7_raw;
ros_rc_msg_.chan8_raw = rc.chan8_raw;
ros_rc_msg_.rssi = rc.rssi;
rc_pub_.publish(ros_rc_msg_);
}
}
break;
/* case MAVLINK_MSG_ID_G: //msg ID: 28
{
mavlink_raw_pressure_t baro;
}
break; */
case MAVLINK_MSG_ID_RAW_PRESSURE: //msg ID: 28
{
mavlink_raw_pressure_t baro;
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
// EXECUTE ACTION
break;
default:
//Do nothing
break;
}
}
}
// my_serial.flush();
}
/*Mainloop*/
void loop() {
/*Auswertung des anstehenden Aufgaben im Scheduler*/
int i; unsigned char new_value = 0; unsigned char new_heading = 0;
for (i = 0; i < MAX_TASKS; i++) {
if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;}
switch (scheduler[i].task) {
case NOTHING: break;
case MEASURE:
/*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/
srf[scheduler[i].param - SE0_ADDRESS].measure();
schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param);
scheduler[i].task = NOTHING;
break;
case READ_MEASURE: {
unsigned short index = scheduler[i].param - SE0_ADDRESS;
/*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/
srf[index].read_it(get_current_millis());
schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param);
if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) {
nvalue[index] = 1;
}
/*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) {
nvalue2[scheduler[i].param - SE0_ADDRESS] = 1;
}*/
new_value = scheduler[i].param;
/*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/
if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
/*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/
static unsigned char rm_state[SE_COUNT];
static short rm_min[SE_COUNT];
if (state != IDLE && state != INIT && state != DELAY) {
if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) {
rm_state[index] = 1;
if (index == 0 || index == 2) rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF;
else rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF;
srf[index].set_mean(SE_MIN);
} else if (rm_state[index] == 1) {
if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) {
rm_state[index] = 0;
} else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) {
rm_state[index] = 0;
} else {
srf[index].set_mean(SE_MIN);
}
}
}
if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;}
if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;}
if (state == HOLD_STILL || state == GET_ANCHOR || state == HTM_DELAY) {
if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) {
slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis());
nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0;
}
}
/*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/
#if LOG > 0
FILE *fd;
if (scheduler[i].param == SE0_ADDRESS) fd = fd_112;
else if (scheduler[i].param == SE1_ADDRESS) fd = fd_113;
else if (scheduler[i].param == SE2_ADDRESS) fd = fd_114;
else fd = fd_115;
fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean());
#endif
scheduler[i].task = NOTHING;
break;
}
case SAVE_LOG:
/*Sichert die bisher geloggten Daten*/
#if LOG > 0
fclose(fd_112);
fclose(fd_113);
fclose(fd_114);
fclose(fd_115);
fclose(fd_data);
char tmp_dir[32]; strcpy(tmp_dir,log_dir);
fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir);
fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir);
fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir);
fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir);
fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir);
schedule_add(LOG_SPEED,SAVE_LOG,0);
#endif
scheduler[i].task = NOTHING;
break;
case CHECK_STILL:
/*Prueft auf Stillstand des Copters*/
if (still && state == HOLD_STILL) {
if (scheduler[i].param < CHECK_STILL_COUNT) {
schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1);
} else {
state = MEASURE_ENVIRONMENT;
still = 0;
schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0);
}
} else {
schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0);
}
scheduler[i].task = NOTHING;
break;
case CHANGE_STATE:
/*Ändert den aktuellen Status zum als Parameter übergebenen Status*/
state = (states)scheduler[i].param;
if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0;
scheduler[i].task = NOTHING;
break;
case SHOW_ME: {
/*Schreibt Messwerte und Neigungswinkel auf das Terminal*/
char st[16];
switch (state) {
case INIT: sprintf(st, "INIT"); break;
case MEASURE_ENVIRONMENT: sprintf(st, "ME"); break;
case IDLE: sprintf(st, "IDLE"); break;
case HOLD_STILL: sprintf(st, "HOLD_STILL"); break;
case GET_ALIGNMENT: sprintf(st, "GET_ALIGNMENT"); break;
case GET_ANCHOR: sprintf(st, "GET_ANCHOR"); break;
case DELAY: sprintf(st, "DELAY"); break;
default: sprintf(st, "???"); break;
}
if (roll > 9 || roll < 0) printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
else printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
schedule_add(100,SHOW_ME,0);
scheduler[i].task = NOTHING;
/*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/
mavlink_message_t msg;
uint16_t len;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
static int i; i++;
if (i == 10) {
i = 0;
mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
}
mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data());
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
break;
}
default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break;
}
}
/*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/
struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0;
/*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/
FD_ZERO(&tty_fdset);
FD_SET(tty_fd, &tty_fdset);
if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);}
/*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/
if (FD_ISSET(tty_fd, &tty_fdset)) {
static mavlink_message_t msg;
static mavlink_status_t status;
char c[1];
if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}}
#if DEBUG_LEVEL > 2
printf("%#x\n",c[0]);
#endif
if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) {
#if DEBUG_LEVEL > 1
printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
#endif
//printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
/*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(&msg,&hb);
if (!first_heartbeat) {
first_heartbeat = 1;
request_data_stream(MAV_DATA_STREAM_ALL,0,0);
}
//if (state == IDLE) {
if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) {
std::cout << "State changed from IDLE to INIT.\n";
state = INIT;
schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1);
init_state = 1;
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
/*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/
mavlink_vfr_hud_t hud_data;
mavlink_msg_vfr_hud_decode(&msg, &hud_data);
old_heading = current_heading;
current_heading = hud_data.heading;
new_heading = 1;
break;
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_attitude_t adata;
mavlink_msg_attitude_decode(&msg,&adata);
old_heading = current_heading;
current_heading = ((short)lround(DEG(adata.yaw))+360)%360;
current_pitch_rad = adata.pitch;
current_roll_rad = adata.roll;
new_heading = 1;
break;
case MAVLINK_MSG_ID_RAW_IMU:
/*Speichert die Beschleunigungen auf der x- und y-Achse*/
mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(&msg,&raw_imu);
xacc = raw_imu.xacc;
yacc = raw_imu.yacc;
//slam_insert_acc(xacc,yacc,current_heading,get_current_millis());
std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n";
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
/*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/
/*if (state != IDLE) {break;}
mavlink_rc_channels_raw_t rc;
mavlink_msg_rc_channels_raw_decode(&msg,&rc);
if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1;
if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) {
std::cout << "State changed from IDLE to INIT.\n";
schedule_add(0,CHANGE_STATE,(int)INIT);
schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0);
request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1);
request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1);
}
if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) {
init_state = 1;
}*/
break;
default: break;
}
}
}
/*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/
if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;}
switch (state) {
case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/ break;
case INIT:
std::cout << "INIT\n";
/*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/
schedule_add(5000,CHECK_STILL,0);
#if LOG > 0
schedule_add(LOG_SPEED,SAVE_LOG,0);
//schedule_add(SLAM_SPEED,SAVE_SLAM,0);
#endif
for (int i = 0; i < SE_COUNT; i++) {
if ((int)pow(2,i) & ENABLED_SENSORS) {
schedule_add(0, MEASURE, SE0_ADDRESS + i);
}
}
slam_init(current_heading,get_current_millis());
state = DELAY;
break;
case HOLD_STILL:
if(1) {
static short set_point[SE_COUNT];
if (hs_state == 0) {
hs_state = 1;
set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2;
set_point[1] = set_point[0];
set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2;
set_point[3] = set_point[2];
//for (int i = 0; i < SE_COUNT; i++) {
// set_point[i] = srf[i].get_mean();
// std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
//}
/*for (int i = 0; i < SE_COUNT; i++) {
if (set_point[i] < 100 || set_point[i] > 0) {
if (i == 0 || i == 2) {
set_point[i+1] -= (100 - set_point[i]);
} else {
set_point[i-1] -= (100 - set_point[i]);
}
set_point[i] = 100;
}
}*/
for (int i = 0; i < SE_COUNT; i++) {
std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
}
}
/*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/
if (!new_value) break;
if (nvalue[0] == 1 && nvalue[1] == 1) {
if (srf[0].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll);
else if (srf[1].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll);
else roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll);
nvalue[0] = 0; nvalue[1] = 0;
}
if (nvalue[2] == 1 && nvalue[3] == 1) {
if (srf[2].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch);
else if (srf[3].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch);
else pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch);
nvalue[2] = 0; nvalue[3] = 0;
}
yaw = 0;
send_ext_ctrl();
/*Prüfung auf Stillstand*/
if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) {
//still = 1;
still = 0; /*FIXME*/
} else {
still = 0;
}
}
break;
case MEASURE_ENVIRONMENT:
/*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/
if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;}
if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;}
if (first_heading == -1) {
/*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/
std::cout << "State changed to MEASURE_ENVIRONMENT\n";
first_heading = current_heading;
roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW;
send_ext_ctrl();
}
//if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360);
/*Berechnung der aktuellen Drehung*/
if (new_heading) {
if (abs(current_heading - old_heading) > 180/SE_COUNT) rotation += current_heading - old_heading - sign(current_heading - old_heading)*360;
else rotation += current_heading - old_heading;
for(int i = 0; i < SE_COUNT; i++) {
if(nvalue[i] == 1) {
env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean();
nvalue[i] = 0;
}
}
//if (abs(rotation) >= 360/SE_COUNT) {
if (abs(rotation) >= rotation_angle) {
/*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/
//state = HEAD_TO_MIDDLE;
state = GET_ALIGNMENT;
roll = 0; pitch = 0; yaw = 0;
send_ext_ctrl();
rotation_angle_sign = sign(rotation);
}
}
break;
case GET_ALIGNMENT: {
/*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/
/*short min_v[SE_COUNT];
short min_h[SE_COUNT];
for (int i = 0; i < SE_COUNT; i++) {
min_v[i] = 0;
min_h[i] = 0;
}*/
short min_v = 0;
for (int i = 0; i < rotation_angle; i++) {
if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) {
min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360];
desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360;
}
/*for (int j = 0; j < SE_COUNT; j++) {
if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) {
min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360];
min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360;
}
}*/
}
//desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4);
/*Bestimmung der künftigen Drehrichtung*/
for (int k = 0; k < rotation_angle/2; k++) {
/*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/
if (desired_heading == (first_heading+rotation_angle_sign*k)%360) {
rotation_angle_sign *= -1;
if (init_state == 2) init_state = 3;
break;
}
}
/*Cleanup*/
first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset();
for(int i = 0; i < 360; i++) env[i] = 0;
if (init_state == 1) init_state = 2;
if (init_state == 3) {
state = GET_ANCHOR;
pid_yaw.reset();
pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV);
pid_yaw.set_target(0);
roll = 0; pitch = 0; yaw = 0;
std::cout << "State changed to GET_ANCHOR\n";
tv_old_heading = get_current_millis();
} else {
state = HOLD_STILL;
hs_state = 0;
}
break;
}
case GET_ANCHOR:
/*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/
if (new_heading) {
short heading_diff;
/*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/
if (abs(desired_heading - current_heading) > 180) {
heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360;
} else {
heading_diff = desired_heading - current_heading;
}
yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading);
tv_old_heading = get_current_millis();
}
if (new_value) {
roll = 0; pitch = 0;
if (nvalue[align_se] == 1) {
if (align_se == 0) roll = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll);
else if (align_se == 1) roll = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll);
else if (align_se == 2) pitch = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch);
else if (align_se == 3) pitch = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch);
nvalue[align_se] = 0;
}
if (srf[0].get_mean() < 30) roll = 1000;
else if (srf[1].get_mean() < 30) roll = -1000;
else if (srf[2].get_mean() < 30) pitch = 1000;
else if (srf[3].get_mean() < 30) pitch = -1000;
if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) {
for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0;
/*state = MOVE_BACKWARD*/
/*TODO Neuausrichtung des Copters?*/
}
}
if (breakpoint == 3) {roll = 0; pitch = 0; send_ext_ctrl();}
else if (breakpoint != 2) {send_ext_ctrl();}
break;
case DELAY: break;
case HTM_DELAY: break;
default: break;
}
#if LOG > 0
fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad);
#endif
}
void mavlink_handleMessage(mavlink_message_t* msg) {
mavlink_message_t msg2;
char sysmsg_str[1024];
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
droneType = packet.type;
autoPilot = packet.autopilot;
if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
ModelData.mode = MODEL_MODE_MANUAL;
} else if (packet.base_mode == 128 + 64 + 16) {
ModelData.mode = MODEL_MODE_RTL;
} else if (packet.base_mode == 128 + 16) {
ModelData.mode = MODEL_MODE_POSHOLD;
} else if (packet.base_mode == 128 + 4) {
ModelData.mode = MODEL_MODE_MISSION;
}
if (packet.system_status == MAV_STATE_ACTIVE) {
ModelData.armed = MODEL_ARMED;
} else {
ModelData.armed = MODEL_DISARMED;
}
// SDL_Log("Heartbeat: %i, %i, %i\n", ModelData.armed, ModelData.mode, ModelData.status);
ModelData.heartbeat = 100;
// sprintf(sysmsg_str, "Heartbeat: %i", (int)time(0));
if ((*msg).sysid != 0xff) {
ModelData.sysid = (*msg).sysid;
ModelData.compid = (*msg).compid;
if (mavlink_maxparam == 0) {
mavlink_start_feeds();
}
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
mavlink_rc_channels_scaled_t packet;
mavlink_msg_rc_channels_scaled_decode(msg, &packet);
// SDL_Log("Radio: %i,%i,%i\n", packet.chan1_scaled, packet.chan2_scaled, packet.chan3_scaled);
/* if ((int)packet.chan6_scaled > 1000) {
mode = MODE_MISSION;
} else if ((int)packet.chan6_scaled < -1000) {
mode = MODE_MANUEL;
} else {
mode = MODE_POSHOLD;
}
if ((int)packet.chan7_scaled > 1000) {
mode = MODE_RTL;
} else if ((int)packet.chan7_scaled < -1000) {
mode = MODE_SETHOME;
}
*/
ModelData.radio[0] = (int)packet.chan1_scaled / 100.0;
ModelData.radio[1] = (int)packet.chan2_scaled / 100.0;
ModelData.radio[2] = (int)packet.chan3_scaled / 100.0;
ModelData.radio[3] = (int)packet.chan4_scaled / 100.0;
ModelData.radio[4] = (int)packet.chan5_scaled / 100.0;
ModelData.radio[5] = (int)packet.chan6_scaled / 100.0;
ModelData.radio[6] = (int)packet.chan7_scaled / 100.0;
ModelData.radio[7] = (int)packet.chan8_scaled / 100.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
// SDL_Log("BAR;%i;%0.2f;%0.2f;%0.2f\n", time(0), packet.press_abs, packet.press_diff, packet.temperature / 100.0);
// redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
ModelData.roll = toDeg(packet.roll);
ModelData.pitch = toDeg(packet.pitch);
if (toDeg(packet.yaw) < 0.0) {
ModelData.yaw = 360.0 + toDeg(packet.yaw);
} else {
ModelData.yaw = toDeg(packet.yaw);
}
mavlink_update_yaw = 1;
// SDL_Log("ATT;%i;%0.2f;%0.2f;%0.2f\n", time(0), toDeg(packet.roll), toDeg(packet.pitch), toDeg(packet.yaw));
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_IMU: {
// SDL_Log("SCALED_IMU\n");
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(msg, &packet);
if (packet.lat != 0.0) {
GPS_found = 1;
ModelData.p_lat = (float)packet.lat / 10000000.0;
ModelData.p_long = (float)packet.lon / 10000000.0;
ModelData.p_alt = (float)packet.alt / 1000.0;
ModelData.speed = (float)packet.vel / 100.0;
ModelData.numSat = packet.satellites_visible;
ModelData.gpsfix = packet.fix_type;
redraw_flag = 1;
}
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
// SDL_Log("RC_CHANNELS_RAW\n");
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
// SDL_Log("SERVO_OUTPUT_RAW\n");
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t packet;
mavlink_msg_sys_status_decode(msg, &packet);
// SDL_Log("%0.1f %%, %0.3f V)\n", packet.load / 10.0, packet.voltage_battery / 1000.0);
ModelData.voltage = packet.voltage_battery / 1000.0;
ModelData.load = packet.load / 10.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t packet;
mavlink_msg_statustext_decode(msg, &packet);
SDL_Log("mavlink: ## %s ##\n", packet.text);
sys_message((char *)packet.text);
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
char var[101];
uint16_t n1 = 0;
uint16_t n2 = 0;
for (n1 = 0; n1 < strlen(packet.param_id); n1++) {
if (packet.param_id[n1] != 9 && packet.param_id[n1] != ' ' && packet.param_id[n1] != '\t') {
var[n2++] = packet.param_id[n1];
}
}
var[n2++] = 0;
// MAV_VAR_FLOAT=0, /* 32 bit float | */
// MAV_VAR_UINT8=1, /* 8 bit unsigned integer | */
// MAV_VAR_INT8=2, /* 8 bit signed integer | */
// MAV_VAR_UINT16=3, /* 16 bit unsigned integer | */
// MAV_VAR_INT16=4, /* 16 bit signed integer | */
// MAV_VAR_UINT32=5, /* 32 bit unsigned integer | */
// MAV_VAR_INT32=6, /* 32 bit signed integer | */
sprintf(sysmsg_str, "PARAM_VALUE (%i/%i): #%s# = %f (Type: %i)", packet.param_index + 1, packet.param_count, var, packet.param_value, packet.param_type);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
mavlink_maxparam = packet.param_count;
mavlink_timeout = 0;
mavlink_set_value(var, packet.param_value, packet.param_type, packet.param_index);
if (packet.param_index + 1 == packet.param_count || packet.param_index % 10 == 0) {
mavlink_param_xml_meta_load();
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t packet;
mavlink_msg_mission_count_decode(msg, &packet);
sprintf(sysmsg_str, "MISSION_COUNT: %i\n", packet.count);
sys_message(sysmsg_str);
mission_max = packet.count;
if (mission_max > 0) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 0);
mavlink_send_message(&msg2);
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ACK: {
SDL_Log("mavlink: Mission-Transfer ACK\n");
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
uint16_t id = packet.seq;
uint16_t id2 = packet.seq;
uint16_t type = 0;
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (id2 > 0) {
id2 = id2 - 1;
} else {
SDL_Log("mavlink: WORKAROUND: first WP == HOME ?\n");
}
}
sprintf(sysmsg_str, "sending Waypoint (%i): %s\n", id, WayPoints[1 + id2].name);
sys_message(sysmsg_str);
if (strcmp(WayPoints[1 + id2].command, "WAYPOINT") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_WAYPOINT\n");
type = MAV_CMD_NAV_WAYPOINT;
} else if (strcmp(WayPoints[1 + id2].command, "RTL") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
type = MAV_CMD_NAV_RETURN_TO_LAUNCH;
} else if (strcmp(WayPoints[1 + id2].command, "LAND") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_LAND\n");
type = MAV_CMD_NAV_LAND;
} else if (strcmp(WayPoints[1 + id2].command, "TAKEOFF") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_TAKEOFF\n");
type = MAV_CMD_NAV_TAKEOFF;
} else {
SDL_Log("mavlink: Type: UNKNOWN\n");
type = MAV_CMD_NAV_WAYPOINT;
}
sprintf(sysmsg_str, "SENDING MISSION_ITEM: %i: %f, %f, %f\n", id, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
SDL_Log("mavlink: %s\n", sysmsg_str);
mavlink_msg_mission_item_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, id, 0, type, 0.0, 0.0, WayPoints[1 + id2].radius, WayPoints[1 + id2].wait, WayPoints[1 + id2].orbit, WayPoints[1 + id2].yaw, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
mavlink_send_message(&msg2);
/*
mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t packet;
mavlink_msg_mission_item_decode(msg, &packet);
sprintf(sysmsg_str, "RECEIVED MISSION_ITEM: %i/%i: %f, %f, %f (%i)\n", packet.seq, mission_max, packet.x, packet.y, packet.z, packet.frame);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
if (packet.seq < mission_max - 1) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, packet.seq + 1);
mavlink_send_message(&msg2);
} else {
mavlink_msg_mission_ack_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 15);
mavlink_send_message(&msg2);
}
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (packet.seq > 0) {
packet.seq = packet.seq - 1;
} else {
SDL_Log("mavlink: WORKAROUND: ignore first WP\n");
break;
}
}
SDL_Log("mavlink: getting WP(%i): %f, %f\n", packet.seq, packet.x, packet.y);
switch (packet.command) {
case MAV_CMD_NAV_WAYPOINT: {
strcpy(WayPoints[1 + packet.seq].command, "WAYPOINT");
break;
}
case MAV_CMD_NAV_LOITER_UNLIM: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_UNLIM");
break;
}
case MAV_CMD_NAV_LOITER_TURNS: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TURNS");
break;
}
case MAV_CMD_NAV_LOITER_TIME: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TIME");
break;
}
case MAV_CMD_NAV_RETURN_TO_LAUNCH: {
strcpy(WayPoints[1 + packet.seq].command, "RTL");
break;
}
case MAV_CMD_NAV_LAND: {
strcpy(WayPoints[1 + packet.seq].command, "LAND");
break;
}
case MAV_CMD_NAV_TAKEOFF: {
strcpy(WayPoints[1 + packet.seq].command, "TAKEOFF");
break;
}
default: {
sprintf(WayPoints[1 + packet.seq].command, "CMD:%i", packet.command);
break;
}
}
if (packet.x == 0.0) {
packet.x = 0.00001;
}
if (packet.y == 0.0) {
packet.y = 0.00001;
}
if (packet.z == 0.0) {
packet.z = 0.00001;
}
WayPoints[1 + packet.seq].p_lat = packet.x;
WayPoints[1 + packet.seq].p_long = packet.y;
WayPoints[1 + packet.seq].p_alt = packet.z;
WayPoints[1 + packet.seq].yaw = packet.param4;
sprintf(WayPoints[1 + packet.seq].name, "WP%i", packet.seq + 1);
WayPoints[1 + packet.seq + 1].p_lat = 0.0;
WayPoints[1 + packet.seq + 1].p_long = 0.0;
WayPoints[1 + packet.seq + 1].p_alt = 0.0;
WayPoints[1 + packet.seq + 1].yaw = 0.0;
WayPoints[1 + packet.seq + 1].name[0] = 0;
WayPoints[1 + packet.seq + 1].command[0] = 0;
/*
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="0" name="MAV_FRAME_GLOBAL">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="1" name="MAV_FRAME_LOCAL_NED">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="2" name="MAV_FRAME_MISSION">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="3" name="MAV_FRAME_GLOBAL_RELATIVE_ALT">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="4" name="MAV_FRAME_LOCAL_ENU">
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT: {
mavlink_mission_current_t packet;
mavlink_msg_mission_current_decode(msg, &packet);
// SDL_Log("mavlink: ## Active_WP %f ##\n", packet.seq);
uav_active_waypoint = (uint8_t)packet.seq;
break;
}
case MAVLINK_MSG_ID_RAW_IMU: {
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
/*
SDL_Log("## IMU_RAW_ACC_X %i ##\n", packet.xacc);
SDL_Log("## IMU_RAW_ACC_Y %i ##\n", packet.yacc);
SDL_Log("## IMU_RAW_ACC_Z %i ##\n", packet.zacc);
SDL_Log("## IMU_RAW_GYRO_X %i ##\n", packet.xgyro);
SDL_Log("## IMU_RAW_GYRO_Y %i ##\n", packet.ygyro);
SDL_Log("## IMU_RAW_GYRO_Z %i ##\n", packet.zgyro);
SDL_Log("## IMU_RAW_MAG_X %i ##\n", packet.xmag);
SDL_Log("## IMU_RAW_MAG_Y %i ##\n", packet.ymag);
SDL_Log("## IMU_RAW_MAG_Z %i ##\n", packet.zmag);
*/
ModelData.acc_x = (float)packet.xacc / 1000.0;
ModelData.acc_y = (float)packet.yacc / 1000.0;
ModelData.acc_z = (float)packet.zacc / 1000.0;
ModelData.gyro_x = (float)packet.zgyro;
ModelData.gyro_y = (float)packet.zgyro;
ModelData.gyro_z = (float)packet.zgyro;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t packet;
mavlink_msg_nav_controller_output_decode(msg, &packet);
/*
nav_roll
nav_pitch
alt_error
aspd_error
xtrack_error
nav_bearing
target_bearing
wp_dist
*/
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t packet;
mavlink_msg_vfr_hud_decode(msg, &packet);
// SDL_Log("## pa %f ##\n", packet.airspeed);
// SDL_Log("## pg %f ##\n", packet.groundspeed);
// SDL_Log("## palt %f ##\n", packet.alt);
if (GPS_found == 0) {
ModelData.p_alt = packet.alt;
}
// SDL_Log("## pc %f ##\n", packet.climb);
// SDL_Log("## ph %i ##\n", packet.heading);
// SDL_Log("## pt %i ##\n", packet.throttle);
break;
}
case MAVLINK_MSG_ID_RADIO: {
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
SDL_Log("mavlink: ## rxerrors %i ##\n", packet.rxerrors);
SDL_Log("mavlink: ## fixed %i ##\n", packet.fixed);
SDL_Log("mavlink: ## rssi %i ##\n", packet.rssi);
SDL_Log("mavlink: ## remrssi %i ##\n", packet.remrssi);
SDL_Log("mavlink: ## txbuf %i ##\n", packet.txbuf);
SDL_Log("mavlink: ## noise %i ##\n", packet.noise);
SDL_Log("mavlink: ## remnoise %i ##\n", packet.remnoise);
break;
}
default: {
// SDL_Log(" ## MSG_ID == %i ##\n", msg->msgid);
break;
}
}
}
