ardupilot/Tools/ArduTracker/GCS_Mavlink.pde

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

#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK

#include "Mavlink_Common.h"

GCS_MAVLINK::GCS_MAVLINK() :
    packet_drops(0)
{
}

void
GCS_MAVLINK::init(BetterStream * port)
{
    GCS_Class::init(port);
    mavlink_comm_1_port = port;
	chan = MAVLINK_COMM_1;
}

void
GCS_MAVLINK::update(void)
{
	// recieve new packets
	mavlink_message_t msg;
    mavlink_status_t status;

    // process received bytes
    while(comm_get_available(chan))
    {
        uint8_t c = comm_receive_ch(chan);

        // Try to get a new message
        if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
    }

    // Update packet drops counter
    packet_drops += status.packet_rx_drop_count;

	// send out queued params/ waypoints
	mavlink_queued_send(chan);

	// send parameters communication_queued_send(chan); 
    // stop waypoint sending if timeout
    if (global_data.waypoint_sending &&
            millis() - global_data.waypoint_timelast_send >
            global_data.waypoint_send_timeout)
    {
		send_text(SEVERITY_LOW,"waypoint send timeout");
        global_data.waypoint_sending = false;
    }

    // stop waypoint receiving if timeout
    if (global_data.waypoint_receiving &&
            millis() - global_data.waypoint_timelast_receive >
            global_data.waypoint_receive_timeout)
    {
		send_text(SEVERITY_LOW,"waypoint receive timeout");
        global_data.waypoint_receiving = false;
    }
}

void 
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
{
    if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax))
        send_message(MSG_RAW_IMU);

    if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax))
        send_message(MSG_EXTENDED_STATUS);

    if (freqLoopMatch(global_data.streamRateRCChannels,freqMin,freqMax))
        send_message(MSG_RADIO_OUT);

    if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax))
        send_message(MSG_SERVO_OUT);

    //if (freqLoopMatch(global_data.streamRateRawSensorFusion,freqMin,freqMax))

    if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax))
        send_message(MSG_LOCATION);

    if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax))
        send_message(MSG_GPS_STATUS);

    if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax))
        send_message(MSG_CURRENT_WAYPOINT);

    if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax))
	{
		send_message(MSG_LOCAL_LOCATION);
		send_message(MSG_ATTITUDE);
	}
}

void
GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
{
	mavlink_send_message(chan,id,param,packet_drops);
}

void
GCS_MAVLINK::send_text(uint8_t severity, const char *str)
{
	mavlink_send_text(chan,severity,str);
}

void
GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
	mavlink_acknowledge(chan,id,sum1,sum2);
}

void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
	switch (msg->msgid)	{

	case MAVLINK_MSG_ID_GLOBAL_POSITION:
	{
    	// decode
        mavlink_global_position_t packet;
        mavlink_msg_global_position_decode(msg, &packet);
        //if (mavlink_check_target(packet.target_system,packet.target_component)) break;
		trackVehicle_loc.id = 0;
		trackVehicle_loc.p1 = 0;
		trackVehicle_loc.alt = packet.alt;
		trackVehicle_loc.lat = packet.lat;
		trackVehicle_loc.lng = packet.lon;
		Serial.println("received global position packet");
	}


    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
    {
        // decode
        mavlink_request_data_stream_t packet;
        mavlink_msg_request_data_stream_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        int freq = 0; // packet frequency

        if (packet.start_stop == 0) freq = 0; // stop sending
        else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending
        else break;

        switch(packet.req_stream_id)
        {
        case MAV_DATA_STREAM_ALL:
            global_data.streamRateRawSensors = freq; 
            global_data.streamRateExtendedStatus = freq; 
            global_data.streamRateRCChannels = freq; 
            global_data.streamRateRawController = freq; 
            global_data.streamRateRawSensorFusion = freq; 
            global_data.streamRatePosition = freq; 
            global_data.streamRateExtra1 = freq; 
            global_data.streamRateExtra2 = freq; 
            global_data.streamRateExtra3 = freq; 
            break;
        case MAV_DATA_STREAM_RAW_SENSORS:
            global_data.streamRateRawSensors = freq; 
            break;
        case MAV_DATA_STREAM_EXTENDED_STATUS:
            global_data.streamRateExtendedStatus = freq; 
            break;
        case MAV_DATA_STREAM_RC_CHANNELS:
            global_data.streamRateRCChannels = freq; 
            break;
        case MAV_DATA_STREAM_RAW_CONTROLLER:
            global_data.streamRateRawController = freq; 
            break;
        case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
            global_data.streamRateRawSensorFusion = freq; 
            break;
        case MAV_DATA_STREAM_POSITION:
            global_data.streamRatePosition = freq; 
            break;
        case MAV_DATA_STREAM_EXTRA1:
            global_data.streamRateExtra1 = freq; 
            break;
        case MAV_DATA_STREAM_EXTRA2:
            global_data.streamRateExtra2 = freq; 
            break;
        case MAV_DATA_STREAM_EXTRA3:
            global_data.streamRateExtra3 = freq; 
            break;
        default:
            break;
        }
    }

    case MAVLINK_MSG_ID_ACTION:
    {
        // decode
        mavlink_action_t packet;
        mavlink_msg_action_decode(msg, &packet);
        if (mavlink_check_target(packet.target,packet.target_component)) break;

        // do action
		gcs.send_text(SEVERITY_LOW,"action received");
        switch(packet.action)
        {

            case MAV_ACTION_LAUNCH:
                set_mode(TAKEOFF);
                break;

            case MAV_ACTION_RETURN:
                set_mode(RTL);
                break;

            case MAV_ACTION_EMCY_LAND:
                set_mode(LAND);
                break;

            case MAV_ACTION_HALT: 
                loiter_at_location();
                break;

            // can't implement due to APM configuration
            // just setting to manual to be safe
            case MAV_ACTION_MOTORS_START:
            case MAV_ACTION_CONFIRM_KILL:
            case MAV_ACTION_EMCY_KILL:
            case MAV_ACTION_MOTORS_STOP:
            case MAV_ACTION_SHUTDOWN: 
                set_mode(MANUAL);
                break;

            case MAV_ACTION_CONTINUE:
                process_next_command();
                break;

            case MAV_ACTION_SET_MANUAL: 
                set_mode(MANUAL);
                break;

            case MAV_ACTION_SET_AUTO:
                set_mode(AUTO);
                break; 

            case MAV_ACTION_STORAGE_READ:
                //read_EEPROM_startup();
                //read_EEPROM_airstart_critical();
                //read_command_index();
                //read_EEPROM_flight_modes();
                break; 

            case MAV_ACTION_STORAGE_WRITE: 
                //save_EEPROM_trims();
                //save_EEPROM_waypoint_info();
                //save_EEPROM_gains();
                //save_command_index();
                //save_pressure_data();
                //save_EEPROM_radio_minmax();
                //save_user_configs();
                //save_EEPROM_flight_modes();
                break;

            case MAV_ACTION_CALIBRATE_RC: break; 
                trim_radio();
                break;
            
            case MAV_ACTION_CALIBRATE_GYRO:
            case MAV_ACTION_CALIBRATE_MAG: 
            case MAV_ACTION_CALIBRATE_ACC: 
            case MAV_ACTION_CALIBRATE_PRESSURE:
            case MAV_ACTION_REBOOT:  // this is a rough interpretation
                startup_IMU_ground();     
                break; 

            case MAV_ACTION_REC_START: break; 
            case MAV_ACTION_REC_PAUSE: break; 
            case MAV_ACTION_REC_STOP: break; 

            case MAV_ACTION_TAKEOFF:
                set_mode(TAKEOFF);
                break; 

            case MAV_ACTION_NAVIGATE:
                set_mode(AUTO);
                break; 

            case MAV_ACTION_LAND:
                set_mode(LAND);
                break; 

            case MAV_ACTION_LOITER:
                set_mode(LOITER);
                break; 

            default: break;
        }
    }
    break;

    case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
    {
		//send_text(SEVERITY_LOW,"waypoint request list");

        // decode
        mavlink_waypoint_request_list_t packet;
        mavlink_msg_waypoint_request_list_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // Start sending waypoints
        mavlink_msg_waypoint_count_send(chan,msg->sysid,
                                        msg->compid,get(PARAM_WP_TOTAL));
        global_data.waypoint_timelast_send = millis();
        global_data.waypoint_sending = true;
        global_data.waypoint_receiving = false;
        global_data.waypoint_dest_sysid = msg->sysid;
        global_data.waypoint_dest_compid = msg->compid;

    }
    break;

    case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
    {
		//send_text(SEVERITY_LOW,"waypoint request");

        // Check if sending waypiont
        if (!global_data.waypoint_sending) break;

        // decode
        mavlink_waypoint_request_t packet;
        mavlink_msg_waypoint_request_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // send waypoint
        tell_command = get_wp_with_index(packet.seq);

        // set frame of waypoint
        uint8_t frame = MAV_FRAME_GLOBAL; // reference frame 
        uint8_t action = MAV_ACTION_NAVIGATE; // action
        uint8_t orbit_direction = 0; // clockwise(0), counter-clockwise(1)
        float orbit = 0; // loiter radius
        float param1 = 0, param2 = 0;

        switch(tell_command.id)
        {

        case CMD_WAYPOINT: // navigate
            action = MAV_ACTION_NAVIGATE; // action
            break;

        case CMD_LOITER_TIME: // loiter
            orbit = get(PARAM_WP_RADIUS); // XXX setting loiter radius as waypoint acceptance radius
            action = MAV_ACTION_LOITER; // action
            param1 = get(PARAM_WP_RADIUS);
            param2 = tell_command.p1*100; // loiter time
            break;

        case CMD_TAKEOFF: // takeoff
            action = MAV_ACTION_TAKEOFF;
            break;

        case CMD_LAND: // land
            action = MAV_ACTION_LAND;
            break; 

        defaut:
			gcs.send_text(SEVERITY_LOW,"command not handled");
            break;
        }

        // time that the mav should loiter in milliseconds
        uint8_t current = 0; // 1 (true), 0 (false)
        if (packet.seq == get(PARAM_WP_INDEX)) current = 1;
        float yaw_dir = 0; // yaw orientation in radians, 0 = north XXX: what does this do?
        uint8_t autocontinue = 1; // 1 (true), 0 (false)
        float x = tell_command.lng/1.0e7; // local (x), global (longitude)
        float y = tell_command.lat/1.0e7; // local (y), global (latitude)
        float z = tell_command.alt/1.0e2; // local (z), global (altitude)
        // note XXX: documented x,y,z order does not match with gps raw
        mavlink_msg_waypoint_send(chan,msg->sysid,
                                  msg->compid,packet.seq,frame,action,
                                  orbit,orbit_direction,param1,param2,current,x,y,z,yaw_dir,autocontinue);

        // update last waypoint comm stamp
        global_data.waypoint_timelast_send = millis();
    }
    break;

    case MAVLINK_MSG_ID_WAYPOINT_ACK:
    {
		//send_text(SEVERITY_LOW,"waypoint ack");

        // decode
        mavlink_waypoint_ack_t packet;
        mavlink_msg_waypoint_ack_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // check for error
        uint8_t type = packet.type; // ok (0), error(1)

        // turn off waypoint send
        global_data.waypoint_sending = false;
    }
    break;

    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
    {
		//send_text(SEVERITY_LOW,"param request list");

        // decode
        mavlink_param_request_list_t packet;
        mavlink_msg_param_request_list_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // Start sending parameters
        global_data.parameter_i = 0;
    }
    break;

    case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
    {
		//send_text(SEVERITY_LOW,"waypoint clear all");

        // decode
        mavlink_waypoint_clear_all_t packet;
        mavlink_msg_waypoint_clear_all_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // clear all waypoints
        uint8_t type = 0; // ok (0), error(1)
        set(PARAM_WP_TOTAL,0);

        // send acknowledgement 3 times to makes sure it is received
        for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);

        break;
    }

    case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
    {
		//send_text(SEVERITY_LOW,"waypoint set current");

        // decode
        mavlink_waypoint_set_current_t packet;
        mavlink_msg_waypoint_set_current_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // set current waypoint
        set(PARAM_WP_INDEX,packet.seq);
		{
			Location temp;	// XXX this is gross
			temp = get_wp_with_index(packet.seq);
			set_next_WP(&temp);
		}
        mavlink_msg_waypoint_current_send(chan,get(PARAM_WP_INDEX));
        break;
    }

    case MAVLINK_MSG_ID_WAYPOINT_COUNT:
    {
		//send_text(SEVERITY_LOW,"waypoint count");

        // decode
        mavlink_waypoint_count_t packet;
        mavlink_msg_waypoint_count_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // start waypoint receiving
        set(PARAM_WP_TOTAL,packet.count);
        if (get(PARAM_WP_TOTAL) > MAX_WAYPOINTS)
            set(PARAM_WP_TOTAL,MAX_WAYPOINTS);
        global_data.waypoint_timelast_receive = millis();
        global_data.waypoint_receiving = true;
        global_data.waypoint_sending = false;
        global_data.waypoint_request_i = 0;
        break;
    }

    case MAVLINK_MSG_ID_WAYPOINT:
    {
        // Check if receiving waypiont
        if (!global_data.waypoint_receiving) break;

        // decode
        mavlink_waypoint_t packet;
        mavlink_msg_waypoint_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component)) break;

        // check if this is the requested waypoint
        if (packet.seq != global_data.waypoint_request_i) break;

        // store waypoint
        uint8_t loadAction = 0; // 0 insert in list, 1 exec now

        switch (packet.frame)
        {
            case MAV_FRAME_GLOBAL:
            {
                tell_command.lng = 1.0e7f*packet.x;
                tell_command.lat = 1.0e7f*packet.y;
                tell_command.alt = packet.z*1.0e2f;
                break;
            }

            case MAV_FRAME_LOCAL: // local (relative to home position)
            {
                tell_command.lng = 1.0e7f*ToDeg(packet.x/
                        (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lng;
                tell_command.lat = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lat;
                tell_command.alt = -packet.z*1.0e2f + home.alt;
                break;
            }
        }

        // defaults
        tell_command.id = CMD_BLANK;

        switch (packet.action)
        {
        
            case MAV_ACTION_TAKEOFF:
            {
                tell_command.id = CMD_TAKEOFF;
                break;
            } 
            case MAV_ACTION_LAND:
            {
                tell_command.id = CMD_LAND;
                break;
            }

            case MAV_ACTION_NAVIGATE:
            {
                tell_command.id = CMD_WAYPOINT;
                break;
            }

            case MAV_ACTION_LOITER:
            {
                tell_command.id = CMD_LOITER_TIME;
                tell_command.p1 = packet.param2/1.0e2;
                break;
            }
        }

        // save waypoint
        set_wp_with_index(tell_command, packet.seq);

        // update waypoint receiving state machine
        global_data.waypoint_timelast_receive = millis();
        global_data.waypoint_request_i++;

        if (global_data.waypoint_request_i == get(PARAM_WP_TOTAL))
        {
			gcs.send_text(SEVERITY_LOW,"flight plane received");
            uint8_t type = 0; // ok (0), error(1)
            mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
            global_data.waypoint_receiving = false;
            set(PARAM_WP_RADIUS,packet.param1); // XXX takes last waypoint radius for all
            //save_EEPROM_waypoint_info();
        }
        break;
    }

    case MAVLINK_MSG_ID_PARAM_SET:
    {
        // decode
        mavlink_param_set_t packet;
        mavlink_msg_param_set_decode(msg, &packet);
        if (mavlink_check_target(packet.target_system,packet.target_component))
            break;

        // set parameter
        const char * key = (const char*) packet.param_id;

        // iterate known parameters
        // XXX linear search; would be better to sort params & use a binary search
        for (uint16_t i = 0; i < global_data.param_count; i++) {

            // compare key with parameter name
            if (!strcmp_P(key, getParamName(i))) {

                // sanity-check the new parameter
                if (!isnan(packet.param_value) &&						// not nan
					!isinf(packet.param_value)) {						// not inf

                    setParamAsFloat(i,packet.param_value);

                    // Report back new value
                    char param_name[ONBOARD_PARAM_NAME_LENGTH];			/// XXX HACK
                    strcpy_P(param_name, getParamName(i));	/// XXX HACK
                    mavlink_msg_param_value_send(chan,
                                                 (int8_t*)param_name,
                                                 getParamAsFloat(i),
												 global_data.param_count,i);
					// call load if required
					if (i >= PARAM_RLL2SRV_P && i <= PARAM_RLL2SRV_IMAX) pidServoRoll.load_gains();
					if (i >= PARAM_PTCH2SRV_P && i <= PARAM_PTCH2SRV_IMAX) pidServoPitch.load_gains();
					if (i >= PARAM_YW2SRV_P && i <= PARAM_YW2SRV_IMAX) pidServoRudder.load_gains();
					if (i >= PARAM_HDNG2RLL_P && i <= PARAM_HDNG2RLL_IMAX) pidNavRoll.load_gains();
					if (i >= PARAM_ARSPD2PTCH_P && i <= PARAM_ARSPD2PTCH_IMAX) pidNavPitchAirspeed.load_gains();
					if (i >= PARAM_ALT2PTCH_P && i <= PARAM_ALT2PTCH_IMAX) pidNavPitchAltitude.load_gains();
					if (i >= PARAM_ENRGY2THR_P && i <= PARAM_ENRGY2THR_IMAX) pidTeThrottle.load_gains();
					if (i >= PARAM_ALT2THR_P && i <= PARAM_ALT2THR_IMAX) pidAltitudeThrottle.load_gains();
                }
                break;
            }
        }
        break;
    } // end case
  	} // end switch
} // end handle mavlink


#endif