ardupilot/ArduCopterMega/Mavlink_Common.h

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

#ifndef Mavlink_Common_H
#define Mavlink_Common_H

#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK

uint16_t system_type = MAV_FIXED_WING;
byte mavdelay = 0;

static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
{
    if (sysid != mavlink_system.sysid)
    {
        return 1;
    }
    else if (compid != mavlink_system.compid)
    {
		gcs.send_text(SEVERITY_LOW,"component id mismatch");
        return 0; // XXX currently not receiving correct compid from gcs
    }
    else return 0; // no error
}


void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
{
    uint64_t timeStamp = micros();
    switch(id) {

    case MSG_HEARTBEAT:
		mavlink_msg_heartbeat_send(
				chan,
				system_type,
				MAV_AUTOPILOT_ARDUPILOTMEGA);
		break;

    case MSG_EXTENDED_STATUS:
    {
        uint8_t mode 	 = MAV_MODE_UNINIT;
        uint8_t nav_mode = MAV_NAV_VECTOR;

		switch(control_mode) {
		case ACRO:
			mode = MAV_MODE_MANUAL;
			break;
		case STABILIZE:
			mode = MAV_MODE_GUIDED;
			break;
		case FBW:
			mode = MAV_MODE_TEST1;
			break;
		case ALT_HOLD:
			mode = MAV_MODE_TEST2;
			break;
		case POSITION_HOLD:
			mode = MAV_MODE_AUTO;
			nav_mode = MAV_NAV_HOLD;
			break;
		case AUTO:
			mode = MAV_MODE_AUTO;
			nav_mode = MAV_NAV_WAYPOINT;
			break;
		case RTL:
			mode = MAV_MODE_AUTO;
			nav_mode = MAV_NAV_RETURNING;
			break;
		case TAKEOFF:
			mode = MAV_MODE_AUTO;
			nav_mode = MAV_NAV_LIFTOFF;
			break;
		case LAND:
			mode = MAV_MODE_AUTO;
			nav_mode = MAV_NAV_LANDING;
			break;
		}
		uint8_t status 		= MAV_STATE_ACTIVE;
        uint8_t motor_block = false;

		mavlink_msg_sys_status_send(
				chan,
				mode,
				nav_mode,
				status,
				load * 1000,
				battery_voltage1 * 1000,
				motor_block,
				packet_drops);
		break;
    }

    case MSG_ATTITUDE:
    {
		Vector3f omega = dcm.get_gyro();
		mavlink_msg_attitude_send(
				chan,
				timeStamp,
				dcm.roll,
				dcm.pitch,
				dcm.yaw,
				omega.x,
				omega.y,
				omega.z);
		break;
	}

    case MSG_LOCATION:
    {
        Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
        mavlink_msg_global_position_int_send(
        		chan,
        		current_loc.lat,
				current_loc.lng,
				current_loc.alt * 10,
				g_gps->ground_speed / 1.0e2 * rot.a.x,
				g_gps->ground_speed / 1.0e2 * rot.b.x,
				g_gps->ground_speed / 1.0e2 * rot.c.x);
        break;
    }

    case MSG_LOCAL_LOCATION:
    {
        Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
        mavlink_msg_local_position_send(
        		chan,
				timeStamp,
				ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
				ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
				(current_loc.alt - home.alt) / 1.0e2,
				g_gps->ground_speed / 1.0e2 * rot.a.x,
				g_gps->ground_speed / 1.0e2 * rot.b.x,
				g_gps->ground_speed / 1.0e2 * rot.c.x);
        break;
    }

    case MSG_GPS_RAW:
    {
        mavlink_msg_gps_raw_send(
				chan,
				timeStamp,
				g_gps->status(),
				g_gps->latitude / 1.0e7,
				g_gps->longitude / 1.0e7,
				g_gps->altitude / 100.0,
				g_gps->hdop,
				0.0,
				g_gps->ground_speed / 100.0,
				g_gps->ground_course / 100.0);
		break;
    }

    case MSG_SERVO_OUT:
    {
        uint8_t rssi = 1;
		// normalized values scaled to -10000 to 10000
		// This is used for HIL.  Do not change without discussing with HIL maintainers
        mavlink_msg_rc_channels_scaled_send(
        		chan,
				g.rc_1.norm_output(),
				g.rc_2.norm_output(),
				g.rc_3.norm_output(),
				g.rc_4.norm_output(),
				0,0,0,0,rssi);
        break;
    }

    case MSG_RADIO_IN:
    {
        uint8_t rssi = 1;
        mavlink_msg_rc_channels_raw_send(
        		chan,
				 g.rc_1.radio_in,
				 g.rc_2.radio_in,
				 g.rc_3.radio_in,
				 g.rc_4.radio_in,
				 g.rc_5.radio_in,
				 g.rc_6.radio_in,
				 g.rc_7.radio_in,
				 g.rc_8.radio_in,
				 rssi);
        break;
    }

    case MSG_RADIO_OUT:
    {
	    mavlink_msg_servo_output_raw_send(
	    		chan,
				motor_out[0],
				motor_out[1],
				motor_out[2],
				motor_out[3],
				0, 0, 0, 0);
	    break;
    }

    case MSG_VFR_HUD:
    {
        mavlink_msg_vfr_hud_send(
        		chan,
				(float)airspeed / 100.0,
				(float)g_gps->ground_speed / 100.0,
				dcm.yaw_sensor,
				current_loc.alt / 100.0,
				climb_rate,
				nav_throttle);
        break;
    }

	#if HIL_MODE != HIL_MODE_ATTITUDE
	case MSG_RAW_IMU:
	{
		Vector3f accel = imu.get_accel();
		Vector3f gyro = imu.get_gyro();
		//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
		//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
		mavlink_msg_raw_imu_send(
				chan,
				timeStamp,
				accel.x * 1000.0 / gravity,
				accel.y * 1000.0 / gravity,
				accel.z * 1000.0 / gravity,
				gyro.x * 1000.0,
				gyro.y * 1000.0,
				gyro.z * 1000.0,
				compass.mag_x,
				compass.mag_y,
				compass.mag_z);

		mavlink_msg_raw_pressure_send(
				chan,
				timeStamp,
				adc.Ch(AIRSPEED_CH),
				barometer.RawPress,
				0,
				0);
		break;
	}
	#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE

    case MSG_GPS_STATUS:
    {
        mavlink_msg_gps_status_send(
        		chan,
        		g_gps->num_sats,
        		NULL,
        		NULL,
        		NULL,
        		NULL,
        		NULL);
        break;
    }

    case MSG_CURRENT_WAYPOINT:
    {
        mavlink_msg_waypoint_current_send(
        		chan,
        		g.waypoint_index);
        break;
    }

    defualt:
        break;
    }
}

void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
	mavlink_msg_statustext_send(
				chan,
				severity,
				(const int8_t*) str);
}

void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
{
}

#endif // mavlink in use

#endif // inclusion guard