diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde new file mode 100644 index 0000000000..5720e9d142 --- /dev/null +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -0,0 +1,790 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK + +#include "Mavlink_Common.h" + +GCS_MAVLINK::GCS_MAVLINK() : +packet_drops(0) +{ +} + +void +GCS_MAVLINK::init(BetterStream * port) +{ + GCS_Class::init(port); + if (port == &Serial) { // to split hil vs gcs + mavlink_comm_0_port = port; + chan = MAVLINK_COMM_0; + } else { + mavlink_comm_1_port = port; + chan = MAVLINK_COMM_1; + } +} + +void +GCS_MAVLINK::update(void) +{ + // receive 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 + _queued_send(); + + // 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); + send_message(MSG_GPS_STATUS); + send_message(MSG_CURRENT_WAYPOINT); + send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working + } + + if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax)) + send_message(MSG_LOCATION); + + if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) { + // This is used for HIL. Do not change without discussing with HIL maintainers + send_message(MSG_SERVO_OUT); + } + + if (freqLoopMatch(global_data.streamRateRCChannels,freqMin,freqMax)) { + send_message(MSG_RADIO_OUT); + send_message(MSG_RADIO_IN); + } + + if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax)) // Use Extra 1 for AHRS info + send_message(MSG_ATTITUDE); + + if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) // Use Extra 3 for additional HIL info + send_message(MSG_VFR_HUD); + + if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)) + { + // Available datastream + } +} + +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_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.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; + } + 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 + 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,g.waypoint_total + 1); // + home + 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; + global_data.requested_interface = chan; + 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 command = tell_command.id; // command + float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; + + param1 = tell_command.p1; + + // time that the mav should loiter in milliseconds + uint8_t current = 0; // 1 (true), 0 (false) + if (packet.seq == g.waypoint_index) current = 1; + uint8_t autocontinue = 1; // 1 (true), 0 (false) + float x = 0, y = 0, z = 0; + + if (command < MAV_CMD_NAV_LAST) { + // command needs scaling + x = tell_command.lat/1.0e7; // local (x), global (longitude) + y = tell_command.lng/1.0e7; // local (y), global (latitude) + z = tell_command.alt/1.0e2; // local (z), global (altitude) + } else { + // command is raw + x = tell_command.lat; + y = tell_command.lng; + z = tell_command.alt; + } + // TODO - Need to add translation for many of the commands > MAV_CMD_NAV_LAST + + // 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,command,current,autocontinue, + param1,param2,param3,param4,x,y,z); + + // 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 - next call to ::update will kick the first one out + + _queued_parameter = AP_Var::first(); + _queued_parameter_index = 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) + g.waypoint_total.set_and_save(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 + g.waypoint_index.set_and_save(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, g.waypoint_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 + if (packet.count > MAX_WAYPOINTS) { + packet.count = MAX_WAYPOINTS; + } + g.waypoint_total.set_and_save(packet.count - 1); + 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 + + // defaults + tell_command.id = packet.command; + + switch (packet.frame) + { + case MAV_FRAME_GLOBAL: + { + tell_command.p1 = packet.param1; // in as byte no conversion + //tell_command.p1 = packet.param1; // in as byte no conversion + tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + break; + } + + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lng; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lat; + tell_command.alt = -packet.z*1.0e2 + home.alt; + break; + } + + case MAV_FRAME_MISSION: + { + if(tell_command.id >= MAV_CMD_NAV_LAST){ + // these are raw values + tell_command.p1 = packet.param1; // in as byte no conversion + tell_command.lat = packet.x; // in as long no conversion + tell_command.lng = packet.y; // in as long no conversion + tell_command.alt = packet.z; // in as int no conversion + } else { + tell_command.p1 = packet.param1; // in as byte no conversion + tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + } + break; + } + + } + + // save waypoint - and prevent re-setting home + if (packet.seq != 0) + 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 > g.waypoint_total) + { + uint8_t type = 0; // ok (0), error(1) + mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); + send_text(SEVERITY_LOW,"flight plan received"); + global_data.waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: + { + AP_Var *vp; + AP_Meta_class::Type_id var_type; + + // 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; + + // find the requested parameter + vp = AP_Var::find(key); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // fetch the variable type ID + var_type = vp->meta_type_id(); + + // handle variables with standard type IDs + if (var_type == AP_Var::k_typeid_float) { + ((AP_Float *)vp)->set(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_float16) { + ((AP_Float16 *)vp)->set(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int32) { + ((AP_Int32 *)vp)->set(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int16) { + ((AP_Int16 *)vp)->set(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int8) { + ((AP_Int8 *)vp)->set(packet.param_value); + } + } + + // Report back new value + char param_name[ONBOARD_PARAM_NAME_LENGTH]; // XXX HACK - need something to return a char * + vp->copy_name(param_name, sizeof(param_name)); + mavlink_msg_param_value_send(chan, + (int8_t*)param_name, + packet.param_value, + _count_parameters(), + -1); // XXX we don't actually know what its index is... + + break; + } // end case + +#if HIL_MODE != HIL_MODE_DISABLED + // This is used both as a sensor and to pass the location + // in HIL_ATTITUDE mode. + case MAVLINK_MSG_ID_GPS_RAW: + { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + packet.v,packet.hdg,0,0); + break; + } + + // Is this resolved? - MAVLink protocol change..... + case MAVLINK_MSG_ID_VFR_HUD: + { + // decode + mavlink_vfr_hud_t packet; + mavlink_msg_vfr_hud_decode(msg, &packet); + + // set airspeed + airspeed = 100*packet.airspeed; + break; + } +#endif +#if HIL_MODE == HIL_MODE_ATTITUDE + case MAVLINK_MSG_ID_ATTITUDE: + { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + break; + } +#endif +#if HIL_MODE == HIL_MODE_SENSORS + + case MAVLINK_MSG_ID_RAW_IMU: + { + // decode + mavlink_raw_imu_t packet; + mavlink_msg_raw_imu_decode(msg, &packet); + + // set imu hil sensors + // TODO: check scaling for temp/absPress + float temp = 70; + float absPress = 1; + // Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); + // Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); + + // rad/sec + Vector3f gyros; + gyros.x = (float)packet.xgyro / 1000.0; + gyros.y = (float)packet.ygyro / 1000.0; + gyros.z = (float)packet.zgyro / 1000.0; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; + + imu.set_gyro(gyros); + + imu.set_accel(accels); + + compass.setHIL(packet.xmag,packet.ymag,packet.zmag); + break; + } + + case MAVLINK_MSG_ID_RAW_PRESSURE: + { + // decode + mavlink_raw_pressure_t packet; + mavlink_msg_raw_pressure_decode(msg, &packet); + + // set pressure hil sensor + // TODO: check scaling + float temp = 70; + barometer.setHIL(temp,packet.press_diff1); + break; + } +#endif // HIL_MODE + } // end switch +} // end handle mavlink + +uint16_t +GCS_MAVLINK::_count_parameters() +{ + // if we haven't cached the parameter count yet... + if (0 == _parameter_count) { + AP_Var *vp; + + vp = AP_Var::first(); + do { + // if a parameter responds to cast_to_float then we are going to be able to report it + if (!isnan(vp->cast_to_float())) { + _parameter_count++; + } + } while (NULL != (vp = vp->next())); + } + return _parameter_count; +} + +AP_Var * +GCS_MAVLINK::_find_parameter(uint16_t index) +{ + AP_Var *vp; + + vp = AP_Var::first(); + while (NULL != vp) { + + // if the parameter is reportable + if (!(isnan(vp->cast_to_float()))) { + // if we have counted down to the index we want + if (0 == index) { + // return the parameter + return vp; + } + // count off this parameter, as it is reportable but not + // the one we want + index--; + } + // and move to the next parameter + vp = vp->next(); + } + return NULL; +} + +/** +* @brief Send low-priority messages at a maximum rate of xx Hertz +* +* This function sends messages at a lower rate to not exceed the wireless +* bandwidth. It sends one message each time it is called until the buffer is empty. +* Call this function with xx Hertz to increase/decrease the bandwidth. +*/ +void +GCS_MAVLINK::_queued_send() +{ + + // Check to see if we are sending parameters + while (NULL != _queued_parameter) { + AP_Var *vp; + float value; + + + + // copy the current parameter and prepare to move to the next + vp = _queued_parameter; + _queued_parameter = _queued_parameter->next(); + + + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + + char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK + vp->copy_name(param_name, sizeof(param_name)); + + mavlink_msg_param_value_send(chan, + (int8_t*)param_name, + value, + _count_parameters(), + _queued_parameter_index); + + _queued_parameter_index++; + break; + } + + + + + } + + // this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write + mavdelay++; + + // request waypoints one by one + // XXX note that this is pan-interface + if (global_data.waypoint_receiving && global_data.requested_interface == chan && + global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz + { + mavlink_msg_waypoint_request_send(chan, + global_data.waypoint_dest_sysid, + global_data.waypoint_dest_compid ,global_data.waypoint_request_i); + mavdelay = 0; + } +} + +#endif +