/* handle a request to change stream rate. Note that copter passes in save==false, as sending mavlink messages on copter on APM2 costs enough that it can cause flight issues, so we don't want the save to happen when the user connects the ground station. */ void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg, bool save) { mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); int16_t 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 return; AP_Int16 *rate = NULL; switch (packet.req_stream_id) { case MAV_DATA_STREAM_ALL: // note that we don't set STREAM_PARAMS - that is internal only for (uint8_t i=0; i<STREAM_PARAMS; i++) { if (save) { streamRates[i].set_and_save_ifchanged(freq); } else { streamRates[i].set(freq); } } break; case MAV_DATA_STREAM_RAW_SENSORS: rate = &streamRates[STREAM_RAW_SENSORS]; break; case MAV_DATA_STREAM_EXTENDED_STATUS: rate = &streamRates[STREAM_EXTENDED_STATUS]; break; case MAV_DATA_STREAM_RC_CHANNELS: rate = &streamRates[STREAM_RC_CHANNELS]; break; case MAV_DATA_STREAM_RAW_CONTROLLER: rate = &streamRates[STREAM_RAW_CONTROLLER]; break; case MAV_DATA_STREAM_POSITION: rate = &streamRates[STREAM_POSITION]; break; case MAV_DATA_STREAM_EXTRA1: rate = &streamRates[STREAM_EXTRA1]; break; case MAV_DATA_STREAM_EXTRA2: rate = &streamRates[STREAM_EXTRA2]; break; case MAV_DATA_STREAM_EXTRA3: rate = &streamRates[STREAM_EXTRA3]; break; } if (rate != NULL) { if (save) { rate->set_and_save_ifchanged(freq); } else { rate->set(freq); } } }
static void MAVLinkRequestDataStream(mavlink_message_t* handle_msg) // MAVLINK_MSG_ID_REQUEST_DATA_STREAM { int16_t freq = 0; // packet frequency mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(handle_msg, &packet); DPRINT("MAVLINK_MSG_ID_REQUEST_DATA_STREAM %u\r\n", handle_msg->msgid); //send_text((const uint8_t*) "Action: Request data stream\r\n"); // QgroundControl sends data stream request to component ID 1, which is not our component for UDB. if (packet.target_system != mavlink_system.sysid) return; if (packet.start_stop == 0) freq = 0; // stop sending else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending else return; if (packet.req_stream_id == MAV_DATA_STREAM_ALL) { // Warning: mavproxy automatically sets all. Do not include all here, it will overide defaults. streamRates[MAV_DATA_STREAM_RAW_SENSORS] = freq; streamRates[MAV_DATA_STREAM_RC_CHANNELS] = freq; } else { if (packet.req_stream_id < MAV_DATA_STREAM_ENUM_END) streamRates[packet.req_stream_id] = freq; } }
void GCS_Mavlink<T>::handle_request_data_stream(mavlink_message_t *msg) { mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); Serial.print("req_message_rate:"); Serial.print(packet.req_message_rate); Serial.print(","); Serial.print("req_stream_id:"); Serial.print(packet.req_stream_id); Serial.print(","); Serial.print("start_stop:"); Serial.print(packet.start_stop); Serial.print(","); }
//@{ void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; switch (chan) { case MAVLINK_COMM_0: { if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) { // Copy to COMM 1 len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { uart1_transmit(buf[i]); } } } break; case MAVLINK_COMM_1: { if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE && msg->msgid != MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { // Copy to COMM 0 len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { uart0_transmit(buf[i]); } break; } } default: break; } switch (msg->msgid) { case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(msg, &mode); // Check if this system should change the mode if (mode.target == (uint8_t)global_data.param[PARAM_SYSTEM_ID]) { sys_set_mode(mode.mode); // Emit current mode mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); } } break; case MAVLINK_MSG_ID_ACTION: { execute_action(mavlink_msg_action_get_action(msg)); //Forwart actions from Xbee to Onboard Computer and vice versa if (chan == MAVLINK_COMM_1) { mavlink_send_uart(MAVLINK_COMM_0, msg); } else if (chan == MAVLINK_COMM_0) { mavlink_send_uart(MAVLINK_COMM_1, msg); } } break; case MAVLINK_MSG_ID_SYSTEM_TIME: { if (!sys_time_clock_get_unix_offset()) { int64_t offset = ((int64_t) mavlink_msg_system_time_get_time_usec( msg)) - (int64_t) sys_time_clock_get_time_usec(); sys_time_clock_set_unix_offset(offset); debug_message_buffer("UNIX offset updated"); } else { // debug_message_buffer("UNIX offset REFUSED"); } } break; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t stream; mavlink_msg_request_data_stream_decode(msg, &stream); switch (stream.req_stream_id) { case 0: // UNIMPLEMENTED break; case 1: // RAW SENSOR DATA global_data.param[PARAM_SEND_SLOT_RAW_IMU] = stream.start_stop; break; case 2: // EXTENDED SYSTEM STATUS global_data.param[PARAM_SEND_SLOT_ATTITUDE] = stream.start_stop; break; case 3: // REMOTE CONTROL CHANNELS global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] = stream.start_stop; break; case 4: // RAW CONTROLLER //global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; //global_data.param[PARAM_SEND_SLOT_DEBUG_3] = stream.start_stop; global_data.param[PARAM_SEND_SLOT_CONTROLLER_OUTPUT] = stream.start_stop; break; case 5: // SENSOR FUSION //LOST IN GROUDNCONTROL // global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; break; case 6: // POSITION global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; break; case 7: // EXTRA1 global_data.param[PARAM_SEND_SLOT_DEBUG_2] = stream.start_stop; break; case 8: // EXTRA2 global_data.param[PARAM_SEND_SLOT_DEBUG_4] = stream.start_stop; break; case 9: // EXTRA3 global_data.param[PARAM_SEND_SLOT_DEBUG_6] = stream.start_stop; break; default: // Do nothing break; } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t set; mavlink_msg_param_request_read_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; if (set.param_id[0] == '\0') { // Choose parameter based on index if (set.param_index < ONBOARD_PARAM_COUNT) { // Report back value mavlink_msg_param_value_send(chan, (int8_t*) global_data.param_name[set.param_index], global_data.param[set.param_index], ONBOARD_PARAM_COUNT, set.param_index); } } else { for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { // Compare if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } // End matching if null termination is reached if (((char) global_data.param_name[i][j]) == '\0') { break; } } // Check if matched if (match) { // Report back value mavlink_msg_param_value_send(chan, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); } } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // Start sending parameters m_parameter_i = 0; } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { // Compare if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } // End matching if null termination is reached if (((char) global_data.param_name[i][j]) == '\0') { break; } } // Check if matched if (match) { // Only write and emit changes if there is actually a difference // AND only write if new value is NOT "not-a-number" // AND is NOT infy if (global_data.param[i] != set.param_value && !isnan(set.param_value) && !isinf(set.param_value)) { global_data.param[i] = set.param_value; // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); mavlink_msg_param_value_send(MAVLINK_COMM_1, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); debug_message_buffer_sprintf("Parameter received param id=%i",i); } } } } } break; case MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET: { mavlink_position_control_setpoint_set_t pos; mavlink_msg_position_control_setpoint_set_decode(msg, &pos); if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1) { // global_data.position_setpoint.x = pos.x; // global_data.position_setpoint.y = pos.y; // global_data.position_setpoint.z = pos.z; debug_message_buffer("Position setpoint updated. OLD?\n"); } else { debug_message_buffer( "Position setpoint recieved but not updated. OLD?\n"); } // Send back a message confirming the new position mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_0, pos.id, pos.x, pos.y, pos.z, pos.yaw); mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1, pos.id, pos.x, pos.y, pos.z, pos.yaw); } break; case MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET: { mavlink_position_control_offset_set_t set; mavlink_msg_position_control_offset_set_decode(msg, &set); //global_data.attitude_setpoint_pos_body_offset.z = set.yaw; //Ball Tracking if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1 && global_data.param[PARAM_POSITION_YAW_TRACKING]==1) { global_data.param[PARAM_POSITION_SETPOINT_YAW] = global_data.attitude.z + set.yaw; mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 92, set.yaw); } } break; case MAVLINK_MSG_ID_SET_CAM_SHUTTER: { // Decode the desired shutter mavlink_set_cam_shutter_t cam; mavlink_msg_set_cam_shutter_decode(msg, &cam); shutter_set(cam.interval, cam.exposure); debug_message_buffer_sprintf("set_cam_shutter. interval %i", cam.interval); } break; case MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL: { uint8_t enable = mavlink_msg_image_trigger_control_get_enable(msg); shutter_control(enable); if (enable) { debug_message_buffer("CAM: Enabling hardware trigger"); } else { debug_message_buffer("CAM: Disabling hardware trigger"); } } break; case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: { mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(msg, &pos); vision_buffer_handle_data(&pos); // Update validity time is done in vision buffer } break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: { mavlink_vicon_position_estimate_t pos; mavlink_msg_vicon_position_estimate_decode(msg, &pos); global_data.vicon_data.x = pos.x; global_data.vicon_data.y = pos.y; global_data.vicon_data.z = pos.z; global_data.state.vicon_new_data=1; // Update validity time global_data.vicon_last_valid = sys_time_clock_get_time_usec(); global_data.state.vicon_ok=1; // //Set data from Vicon into vision filter // global_data.vision_data.ang.x = pos.roll; // global_data.vision_data.ang.y = pos.pitch; // global_data.vision_data.ang.z = pos.yaw; // // global_data.vision_data.pos.x = pos.x; // global_data.vision_data.pos.y = pos.y; // global_data.vision_data.pos.z = pos.z; // // global_data.vision_data.new_data = 1; if (!global_data.state.vision_ok) { //Correct YAW global_data.attitude.z = pos.yaw; //If yaw goes to infy (no idea why) set it to setpoint, next time will be better if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559) { global_data.attitude.z = global_data.yaw_pos_setpoint; debug_message_buffer( "vicon CRITICAL FAULT yaw was bigger than 6 PI! prevented crash"); } } //send the vicon message to UART0 with new timestamp mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_0, global_data.vicon_last_valid, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw); } break; case MAVLINK_MSG_ID_PING: { mavlink_ping_t ping; mavlink_msg_ping_decode(msg, &ping); if (ping.target_system == 0 && ping.target_component == 0) { // Respond to ping uint64_t r_timestamp = sys_time_clock_get_unix_time(); mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp); } } break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: { mavlink_local_position_setpoint_set_t sp; mavlink_msg_local_position_setpoint_set_decode(msg, &sp); if (sp.target_system == global_data.param[PARAM_SYSTEM_ID]) { if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1) { if (sp.x >= global_data.position_setpoint_min.x && sp.y >= global_data.position_setpoint_min.y && sp.z >= global_data.position_setpoint_min.z && sp.x <= global_data.position_setpoint_max.x && sp.y <= global_data.position_setpoint_max.y && sp.z <= global_data.position_setpoint_max.z) { debug_message_buffer("Setpoint accepted and set."); global_data.param[PARAM_POSITION_SETPOINT_X] = sp.x; global_data.param[PARAM_POSITION_SETPOINT_Y] = sp.y; global_data.param[PARAM_POSITION_SETPOINT_Z] = sp.z; if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 0) { // Only update yaw if we are not tracking ball. global_data.param[PARAM_POSITION_SETPOINT_YAW] = sp.yaw; } //check if we want to start or land if (global_data.state.status == MAV_STATE_ACTIVE || global_data.state.status == MAV_STATE_CRITICAL) { if (sp.z > -0.1) { if (!(global_data.state.fly == FLY_GROUNDED || global_data.state.fly == FLY_SINKING || global_data.state.fly == FLY_WAIT_LANDING || global_data.state.fly == FLY_LANDING || global_data.state.fly == FLY_RAMP_DOWN)) { //if setpoint is lower that ground iate landing global_data.state.fly = FLY_SINKING; global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.2;//with lowpass debug_message_buffer( "Sinking for LANDING. (z-sp lower than 10cm)"); } else if (!(global_data.state.fly == FLY_GROUNDED)) { global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.2;//with lowpass } } else if (global_data.state.fly == FLY_GROUNDED && sp.z < -0.50) { //start if we were grounded and get a sp over 0.5m global_data.state.fly = FLY_WAIT_MOTORS; debug_message_buffer( "STARTING wait motors. (z-sp higher than 50cm)"); //set point changed with lowpass, after 5s it will be ok. } } //SINK TO 0.7m if we are critical or emergency if (global_data.state.status == MAV_STATE_EMERGENCY || global_data.state.status == MAV_STATE_CRITICAL) { global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.7;//with lowpass } } else { debug_message_buffer("Setpoint refused. Out of range."); } } else { debug_message_buffer("Setpoint refused. Param setpoint accept=0."); } } } break; default: break; } }
void mavlink_common_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; /* When requesting data streams say we can't send them */ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t cmd; mavlink_msg_request_data_stream_decode(msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); MAVLinkSendMessage(); break; } /* Override channels with RC */ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { mavlink_rc_channels_override_t cmd; mavlink_msg_rc_channels_override_decode(msg, &cmd); #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100; int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2; int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2; int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100; parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw); //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n", // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw); #endif break; } /* When a request is made of the parameters list */ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_params_idx = 0; break; } /* When a request os made for a specific parameter */ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t cmd; mavlink_msg_param_request_read_decode(msg, &cmd); // First check param_index and search for the ID if needed if (cmd.param_index == -1) { cmd.param_index = settings_idx_from_param_id(cmd.param_id); } // Send message only if the param_index was found (Coverity Scan) if (cmd.param_index > -1) { mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[cmd.param_index], settings_get_value(cmd.param_index), MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); MAVLinkSendMessage(); } break; } case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == AC_ID) { int16_t idx = settings_idx_from_param_id(set.param_id); // setting found if (idx >= 0) { // Only write if new value is NOT "not-a-number" // AND is NOT infinity if (set.param_type == MAV_PARAM_TYPE_REAL32 && !isnan(set.param_value) && !isinf(set.param_value)) { DlSetting(idx, set.param_value); // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[idx], settings_get_value(idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); MAVLinkSendMessage(); } } } } break; #ifndef AP /* only for rotorcraft */ case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(msg, &cmd); // Check if this message is for this system if ((uint8_t) cmd.target_system == AC_ID) { uint8_t result = MAV_RESULT_UNSUPPORTED; switch (cmd.command) { case MAV_CMD_NAV_GUIDED_ENABLE: MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_mode(AP_MODE_GUIDED); if (autopilot_mode == AP_MODE_GUIDED) { result = MAV_RESULT_ACCEPTED; } } else { // turn guided mode off - to what? maybe NAV? or MODE_AUTO2? } break; case MAV_CMD_COMPONENT_ARM_DISARM: /* supposed to use this command to arm or SET_MODE?? */ MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_motors_on(TRUE); if (autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } else { autopilot_set_motors_on(FALSE); if (!autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } break; default: break; } // confirm command with result mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result); MAVLinkSendMessage(); } break; } case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(msg, &mode); if (mode.target_system == AC_ID) { MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode); if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { autopilot_set_motors_on(TRUE); } else { autopilot_set_motors_on(FALSE); } if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { autopilot_set_mode(AP_MODE_GUIDED); } else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { autopilot_set_mode(AP_MODE_NAV); } } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { mavlink_set_position_target_local_ned_t target; mavlink_msg_set_position_target_local_ned_decode(msg, &target); // Check if this message is for this system if (target.target_system == AC_ID) { MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask); /* if position and yaw bits are not set to ignored, use only position for now */ if (!(target.type_mask & 0b1110000000100000)) { switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set position target, frame LOCAL_NED\n"); autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_LOCAL_OFFSET_NED: MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n"); autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_BODY_OFFSET_NED: MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n"); autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw); break; default: break; } } else if (!(target.type_mask & 0b0001110000100000)) { /* position is set to ignore, but velocity not */ switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n"); autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw); break; default: break; } } } break; } #endif default: //Do nothing MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); break; } }
void mavlink_common_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; /* When requesting data streams say we can't send them */ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t cmd; mavlink_msg_request_data_stream_decode(msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); MAVLinkSendMessage(); break; } /* Override channels with RC */ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { mavlink_rc_channels_override_t cmd; mavlink_msg_rc_channels_override_decode(msg, &cmd); #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100; int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2; int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2; int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100; parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw); //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n", // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw); #endif break; } /* When a request is made of the parameters list */ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_params_idx = 0; break; } /* When a request os made for a specific parameter */ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t cmd; mavlink_msg_param_request_read_decode(msg, &cmd); // First check param_index and search for the ID if needed if (cmd.param_index == -1) { cmd.param_index = settings_idx_from_param_id(cmd.param_id); } mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[cmd.param_index], settings_get_value(cmd.param_index), MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); MAVLinkSendMessage(); break; } case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == AC_ID) { int16_t idx = settings_idx_from_param_id(set.param_id); // setting found if (idx >= 0) { // Only write if new value is NOT "not-a-number" // AND is NOT infinity if (set.param_type == MAV_PARAM_TYPE_REAL32 && !isnan(set.param_value) && !isinf(set.param_value)) { DlSetting(idx, set.param_value); // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[idx], settings_get_value(idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); MAVLinkSendMessage(); } } } } break; default: //Do nothing //MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); break; } }