// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK #include "Mavlink_Common.h" // use this to prevent recursion during sensor init static bool in_mavlink_delay; GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : packet_drops(0), // parameters // note, all values not explicitly initialised here are zeroed waypoint_send_timeout(1000), // 1 second waypoint_receive_timeout(1000), // 1 second // stream rates _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), // AP_VAR //ref //index, default, name streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), streamRatePosition (&_group, 4, 0, PSTR("POSITION")), streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) { } 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; } _queued_parameter = NULL; } 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 (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout")); waypoint_sending = false; } // stop waypoint receiving if timeout if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout")); waypoint_receiving = false; } } void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ send_message(MSG_RAW_IMU); //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); } if (freqLoopMatch(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 send_message(MSG_NAV_CONTROLLER_OUTPUT); //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); } if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { // sent with GPS read #if HIL_MODE == HIL_MODE_ATTITUDE send_message(MSG_LOCATION); #endif //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); } if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { // This is used for HIL. Do not change without discussing with HIL maintainers send_message(MSG_SERVO_OUT); //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); } if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); } if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info send_message(MSG_ATTITUDE); //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); } if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info send_message(MSG_VFR_HUD); //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ // Available datastream //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } 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::send_text(uint8_t severity, const prog_char_t *str) { mavlink_statustext_t m; uint8_t i; for (i=0; imsgid) { 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: streamRateRawSensors = freq; streamRateExtendedStatus = freq; streamRateRCChannels = freq; streamRateRawController = freq; streamRatePosition = freq; streamRateExtra1 = freq; streamRateExtra2 = freq; //streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. streamRateExtra3 = freq; // Don't save!! break; case MAV_DATA_STREAM_RAW_SENSORS: streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly // we will not continue to broadcast raw sensor data at 50Hz. break; case MAV_DATA_STREAM_EXTENDED_STATUS: //streamRateExtendedStatus.set_and_save(freq); streamRateExtendedStatus = freq; break; case MAV_DATA_STREAM_RC_CHANNELS: streamRateRCChannels = freq; break; case MAV_DATA_STREAM_RAW_CONTROLLER: streamRateRawController = freq; break; //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: // streamRateRawSensorFusion.set_and_save(freq); // break; case MAV_DATA_STREAM_POSITION: streamRatePosition = freq; break; case MAV_DATA_STREAM_EXTRA1: streamRateExtra1 = freq; break; case MAV_DATA_STREAM_EXTRA2: streamRateExtra2 = freq; break; case MAV_DATA_STREAM_EXTRA3: 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; if (in_mavlink_delay) { // don't execute action commands while in sensor // initialisation break; } uint8_t result = 0; // do action send_text_P(SEVERITY_LOW,PSTR("action received: ")); //Serial.println(packet.action); switch(packet.action){ case MAV_ACTION_LAUNCH: //set_mode(TAKEOFF); break; case MAV_ACTION_RETURN: set_mode(RTL); result=1; break; case MAV_ACTION_EMCY_LAND: //set_mode(LAND); break; case MAV_ACTION_HALT: do_loiter_at_location(); result=1; break; /* No mappable implementation in APM 2.0 case MAV_ACTION_MOTORS_START: case MAV_ACTION_CONFIRM_KILL: case MAV_ACTION_EMCY_KILL: case MAV_ACTION_MOTORS_STOP: case MAV_ACTION_SHUTDOWN: break; */ case MAV_ACTION_CONTINUE: process_next_command(); result=1; break; case MAV_ACTION_SET_MANUAL: set_mode(STABILIZE); result=1; break; case MAV_ACTION_SET_AUTO: set_mode(AUTO); result=1; break; case MAV_ACTION_STORAGE_READ: AP_Var::load_all(); result=1; break; case MAV_ACTION_STORAGE_WRITE: AP_Var::save_all(); result=1; break; case MAV_ACTION_CALIBRATE_RC: break; trim_radio(); result=1; break; case MAV_ACTION_CALIBRATE_GYRO: case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_PRESSURE: break; case MAV_ACTION_CALIBRATE_ACC: imu.init_accel(mavlink_delay); result=1; break; //case MAV_ACTION_REBOOT: // this is a rough interpretation //startup_IMU_ground(); //result=1; // break; /* For future implemtation case MAV_ACTION_REC_START: break; case MAV_ACTION_REC_PAUSE: break; case MAV_ACTION_REC_STOP: break; */ /* Takeoff is not an implemented flight mode in APM 2.0 case MAV_ACTION_TAKEOFF: set_mode(TAKEOFF); break; */ case MAV_ACTION_NAVIGATE: set_mode(AUTO); result=1; break; /* Land is not an implemented flight mode in APM 2.0 case MAV_ACTION_LAND: set_mode(LAND); break; */ case MAV_ACTION_LOITER: set_mode(LOITER); result=1; break; default: break; } mavlink_msg_action_ack_send( chan, packet.action, result ); break; } case MAVLINK_MSG_ID_SET_MODE: { // decode mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg, &packet); switch(packet.mode){ case MAV_MODE_MANUAL: set_mode(STABILIZE); break; case MAV_MODE_GUIDED: set_mode(GUIDED); break; case MAV_MODE_AUTO: if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); mav_nav = 255; break; case MAV_MODE_TEST1: set_mode(STABILIZE); break; } } /*case MAVLINK_MSG_ID_SET_NAV_MODE: { // decode mavlink_set_nav_mode_t packet; mavlink_msg_set_nav_mode_decode(msg, &packet); // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message mav_nav = packet.nav_mode; break; } */ case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { //send_text_P(SEVERITY_LOW,PSTR("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 waypoint_timelast_send = millis(); waypoint_sending = true; waypoint_receiving = false; waypoint_dest_sysid = msg->sysid; waypoint_dest_compid = msg->compid; requested_interface = chan; break; } // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); // Check if sending waypiont //if (!waypoint_sending) break; // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW // 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_command_with_index(packet.seq); // set frame of waypoint uint8_t frame; if (tell_command.options & WP_OPTION_ALT_RELATIVE) { frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame } else { frame = MAV_FRAME_GLOBAL; // reference frame } float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) if (packet.seq == (uint16_t)g.waypoint_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) float x = 0, y = 0, z = 0; if (tell_command.id < MAV_CMD_NAV_LAST) { // command needs scaling x = tell_command.lat/1.0e7; // local (x), global (latitude) y = tell_command.lng/1.0e7; // local (y), global (longitude) // ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) } switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_CONDITION_CHANGE_ALT: case MAV_CMD_DO_SET_HOME: param1 = tell_command.p1; break; case MAV_CMD_NAV_TAKEOFF: param1 = 0; break; case MAV_CMD_NAV_LOITER_TIME: param1 = tell_command.p1; // ACM loiter time is in 1 second increments break; case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DISTANCE: param1 = tell_command.lat; break; case MAV_CMD_DO_JUMP: param2 = tell_command.lat; param1 = tell_command.p1; break; case MAV_CMD_DO_REPEAT_SERVO: param4 = tell_command.lng; case MAV_CMD_DO_REPEAT_RELAY: case MAV_CMD_DO_CHANGE_SPEED: param3 = tell_command.lat; param2 = tell_command.alt; param1 = tell_command.p1; break; case MAV_CMD_NAV_WAYPOINT: param1 = tell_command.p1; break; case MAV_CMD_DO_SET_PARAMETER: case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_SET_SERVO: param2 = tell_command.alt; param1 = tell_command.p1; break; } mavlink_msg_waypoint_send(chan,msg->sysid, msg->compid, packet.seq, frame, tell_command.id, current, autocontinue, param1, param2, param3, param4, x, y, z); // update last waypoint comm stamp waypoint_timelast_send = millis(); break; } case MAVLINK_MSG_ID_WAYPOINT_ACK: { //send_text_P(SEVERITY_LOW,PSTR("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; // turn off waypoint send waypoint_sending = false; break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { //send_text_P(SEVERITY_LOW,PSTR("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; _queued_parameter_count = _count_parameters(); requested_interface = chan; break; } case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { //send_text_P(SEVERITY_LOW,PSTR("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_P(SEVERITY_LOW,PSTR("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 command change_command(packet.seq); mavlink_msg_waypoint_current_send(chan, g.waypoint_index); break; } case MAVLINK_MSG_ID_WAYPOINT_COUNT: { //send_text_P(SEVERITY_LOW,PSTR("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); waypoint_timelast_receive = millis(); waypoint_receiving = true; waypoint_sending = false; waypoint_request_i = 0; break; } // XXX receive a WP from GCS and store in EEPROM case MAVLINK_MSG_ID_WAYPOINT: { // decode mavlink_waypoint_t packet; mavlink_msg_waypoint_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // defaults tell_command.id = packet.command; /* switch (packet.frame){ case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { 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 tell_command.options = 0; 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.lat; tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; tell_command.alt = packet.z*1.0e2; tell_command.options = 1; break; } //case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude default: { 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; tell_command.options = 1; // store altitude relative!! Always!! break; } } */ // we only are supporting Abs position, relative Alt 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; tell_command.options = 1; // store altitude relative!! Always!! switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_DO_SET_HOME: case MAV_CMD_DO_SET_ROI: tell_command.p1 = packet.param1; break; case MAV_CMD_NAV_TAKEOFF: tell_command.p1 = 0; break; case MAV_CMD_CONDITION_CHANGE_ALT: tell_command.p1 = packet.param1 * 100; break; case MAV_CMD_NAV_LOITER_TIME: tell_command.p1 = packet.param1; // APM loiter time is in ten second increments break; case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DISTANCE: tell_command.lat = packet.param1; break; case MAV_CMD_DO_JUMP: tell_command.lat = packet.param2; tell_command.p1 = packet.param1; break; case MAV_CMD_DO_REPEAT_SERVO: tell_command.lng = packet.param4; case MAV_CMD_DO_REPEAT_RELAY: case MAV_CMD_DO_CHANGE_SPEED: tell_command.lat = packet.param3; tell_command.alt = packet.param2; tell_command.p1 = packet.param1; break; case MAV_CMD_NAV_WAYPOINT: tell_command.p1 = packet.param1; break; case MAV_CMD_DO_SET_PARAMETER: case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_SET_SERVO: tell_command.alt = packet.param2; tell_command.p1 = packet.param1; break; } if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission guided_WP = tell_command; // add home alt if needed if (guided_WP.options & WP_OPTION_ALT_RELATIVE){ guided_WP.alt += home.alt; } set_mode(GUIDED); // make any new wp uploaded instant (in case we are already in Guided mode) set_next_WP(&guided_WP); // verify we recevied the command mavlink_msg_waypoint_ack_send( chan, msg->sysid, msg->compid, 0); } else { // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) break; // check if this is the requested waypoint if (packet.seq != waypoint_request_i) break; set_command_with_index(tell_command, packet.seq); // update waypoint receiving state machine waypoint_timelast_receive = millis(); waypoint_request_i++; if (waypoint_request_i > (uint16_t)g.waypoint_total){ uint8_t type = 0; // ok (0), error(1) mavlink_msg_waypoint_ack_send( chan, msg->sysid, msg->compid, type); send_text_P(SEVERITY_LOW,PSTR("flight plan received")); 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 char key[ONBOARD_PARAM_NAME_LENGTH+1]; strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); key[ONBOARD_PARAM_NAME_LENGTH] = 0; // 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 // add a small amount before casting parameter values // from float to integer to avoid truncating to the // next lower integer value. float rounding_addition = 0.01; // 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_and_save(packet.param_value); } else if (var_type == AP_Var::k_typeid_float16) { ((AP_Float16 *)vp)->set_and_save(packet.param_value); } else if (var_type == AP_Var::k_typeid_int32) { if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); } else if (var_type == AP_Var::k_typeid_int16) { if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); } else if (var_type == AP_Var::k_typeid_int8) { if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); } else { // we don't support mavlink set on this parameter break; } // Report back the new value if we accepted the change // we send the value we actually set, which could be // different from the value sent, in case someone sent // a fractional value to an integer type mavlink_msg_param_value_send( chan, (int8_t *)key, vp->cast_to_float(), _count_parameters(), -1); // XXX we don't actually know what its index is... } break; } // end case case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; v[3] = packet.chan4_raw; v[4] = packet.chan5_raw; v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; APM_RC.setHIL(v); break; } #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); if (gps_base_alt == 0) { gps_base_alt = packet.alt*100; } current_loc.lng = packet.lon * T7; current_loc.lat = packet.lat * T7; current_loc.alt = g_gps->altitude - gps_base_alt; if (!home_is_set) { init_home(); } break; } #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 #endif /* case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if(msg->sysid != g.sysid_my_gcs) break; rc_override_fs_timer = millis(); //pmTest1++; break; } #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 if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) { 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, _queued_parameter_count, _queued_parameter_index); _queued_parameter_index++; } mavdelay = 0; } // 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 (waypoint_receiving && (requested_interface == (unsigned)chan) && waypoint_request_i <= (unsigned)g.waypoint_total && mavdelay > 15) { // limits to 3.33 hz mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, waypoint_dest_compid, waypoint_request_i); mavdelay = 0; } } #endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK static void send_rate(uint16_t low, uint16_t high) { #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK gcs.data_stream_send(low, high); #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.data_stream_send(low,high); #endif } /* a delay() callback that processes MAVLink packets. We set this as the callback in long running library initialisation routines to allow MAVLink to process packets while waiting for the initialisation to complete */ static void mavlink_delay(unsigned long t) { unsigned long tstart; static unsigned long last_1hz, last_3hz, last_10hz, last_50hz; if (in_mavlink_delay) { // this should never happen, but let's not tempt fate by // letting the stack grow too much delay(t); return; } in_mavlink_delay = true; tstart = millis(); do { unsigned long tnow = millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs.send_message(MSG_HEARTBEAT); #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_HEARTBEAT); #endif send_rate(1, 3); } if (tnow - last_3hz > 333) { last_3hz = tnow; send_rate(3, 5); } if (tnow - last_10hz > 100) { last_10hz = tnow; send_rate(5, 45); } if (tnow - last_50hz > 20) { last_50hz = tnow; gcs.update(); #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.update(); #endif send_rate(45, 1000); } delay(1); } while (millis() - tstart < t); in_mavlink_delay = false; }