/** \brief This static method is called by the GroundLink's MAVLink component whenever * a new message has been received. It takes the appropriate actions depending on the message type. */ void GroundLink::mavlinkMessageHandler(mavlink_message_t* msg){ // Structures to keep decoded messages. They are declared as static because this way // memory is allocated only once and not each time the method gets called. static mavlink_param_request_list_t parameterListRequestMessage; static mavlink_param_set_t parameterSetMessage; GroundLink* instance = GroundLink::instance; // The instance of GroundLink this static method is operating on. instance->timeOfLastMessage = TimeBase::getSystemTimeMs(); // This may be used to detect connection timeouts. switch(msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: mavlink_msg_param_request_list_decode(msg, ¶meterListRequestMessage); // Check if this message is for us: if(instance->mavLink.isMySystemID(parameterListRequestMessage.target_system)){ // set up counter in order to transmit the first parameter first: instance->linkState.parameterTransactionManager.txCounter = 0; instance->linkState.parameterTransactionManager.state = PAR_LIST_REQUEST_RECEIVED; } break; case MAVLINK_MSG_ID_PARAM_SET: mavlink_msg_param_set_decode(msg, ¶meterSetMessage); // Check if this message is for us: if(instance->mavLink.isMySystemID(parameterSetMessage.target_system)){ // Assign value to onboard parameter. instance->linkState.parameterTransactionManager.indexOfLastReceivedParameter = instance->parameterManager->setFloat(parameterSetMessage.param_value, parameterSetMessage.param_id); instance->linkState.parameterTransactionManager.state = PAR_GOT_PARAMETER; } break; default: break; } }
void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg) { mavlink_param_request_list_t packet; mavlink_msg_param_request_list_decode(msg, &packet); // send system ID if we can char sysid[40]; if (hal.util->get_system_id(sysid)) { send_text(MAV_SEVERITY_WARNING, sysid); } // Start sending parameters - next call to ::update will kick the first one out _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type); _queued_parameter_index = 0; _queued_parameter_count = AP_Param::count_parameters(); }
static void onboard_parameters_send_all_parameters(onboard_parameters_t* onboard_parameters, uint32_t sysid, mavlink_message_t* msg) { mavlink_param_request_list_t packet; mavlink_msg_param_request_list_decode(msg, &packet); if ((uint8_t)packet.target_system == (uint8_t)sysid) { onboard_parameters_set_t* param_set = onboard_parameters->param_set; // schedule all parameters for transmission for (uint8_t i = 0; i < param_set->param_count; i++) { param_set->parameters[i].schedule_for_transmission=true; } } }
void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg) { mavlink_param_request_list_t packet; mavlink_msg_param_request_list_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) { return; } #if CONFIG_HAL_BOARD != HAL_BOARD_APM1 && CONFIG_HAL_BOARD != HAL_BOARD_APM2 // send system ID if we can char sysid[40]; if (hal.util->get_system_id(sysid)) { send_text(SEVERITY_LOW, sysid); } #endif // Start sending parameters - next call to ::update will kick the first one out _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type); _queued_parameter_index = 0; _queued_parameter_count = _count_parameters(); }
void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* request all parameters */ mavlink_param_request_list_t req_list; mavlink_msg_param_request_list_decode(msg, &req_list); if (req_list.target_system == mavlink_system.sysid && (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { _send_all_index = 0; _mavlink->send_statustext_info("[pm] sending list"); } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); if (set.target_system == mavlink_system.sysid && (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ param_t param = param_find(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown param: %s", name); _mavlink->send_statustext_info(buf); } else { /* set and send parameter */ param_set(param, &(set.param_value)); send_param(param); } } } break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { /* request one parameter */ mavlink_param_request_read_t req_read; mavlink_msg_param_request_read_decode(msg, &req_read); if (req_read.target_system == mavlink_system.sysid && (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ send_param(param_find(name)); } else { /* when index is >= 0, send this parameter again */ send_param(param_for_index(req_read.param_index)); } } break; } default: break; } }
void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* request all parameters */ mavlink_param_request_list_t req_list; mavlink_msg_param_request_list_decode(msg, &req_list); if (req_list.target_system == mavlink_system.sysid && (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { _send_all_index = 0; _mavlink->send_statustext_info("[pm] sending list"); } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); if (set.target_system == mavlink_system.sysid && (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ param_t param = param_find(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown param: %s", name); _mavlink->send_statustext_info(buf); } else { /* set and send parameter */ param_set(param, &(set.param_value)); send_param(param); } } } break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { /* request one parameter */ mavlink_param_request_read_t req_read; mavlink_msg_param_request_read_decode(msg, &req_read); if (req_read.target_system == mavlink_system.sysid && (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ send_param(param_find(name)); } else { /* when index is >= 0, send this parameter again */ send_param(param_for_index(req_read.param_index)); } } break; } case MAVLINK_MSG_ID_PARAM_MAP_RC: { /* map a rc channel to a parameter */ mavlink_param_map_rc_t map_rc; mavlink_msg_param_map_rc_decode(msg, &map_rc); if (map_rc.target_system == mavlink_system.sysid && (map_rc.target_component == mavlink_system.compid || map_rc.target_component == MAV_COMP_ID_ALL)) { /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ size_t i = map_rc.parameter_rc_channel_index; _rc_param_map.param_index[i] = map_rc.param_index; strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; _rc_param_map.value_min[i] = map_rc.param_value_min; _rc_param_map.value_max[i] = map_rc.param_value_max; if (map_rc.param_index == -2) { // -2 means unset map _rc_param_map.valid[i] = false; } else { _rc_param_map.valid[i] = true; } _rc_param_map.timestamp = hrt_absolute_time(); if (_rc_param_map_pub < 0) { _rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map); } else { orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map); } } break; } default: break; } }
void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* request all parameters */ mavlink_param_request_list_t req_list; mavlink_msg_param_request_list_decode(msg, &req_list); if (req_list.target_system == mavlink_system.sysid && (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { if (_send_all_index < 0) { _send_all_index = PARAM_HASH; } else { /* a restart should skip the hash check on the ground */ _send_all_index = 0; } } if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { // publish list request to UAVCAN driver via uORB. uavcan_parameter_request_s req; req.message_type = msg->msgid; req.node_id = req_list.target_component; req.param_index = 0; if (_uavcan_parameter_request_pub == nullptr) { _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); } else { orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); } } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); if (set.target_system == mavlink_system.sysid && (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* Whatever the value is, we're being told to stop sending */ if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) { _send_all_index = -1; /* No other action taken, return */ return; } /* attempt to find parameter, set and send it */ param_t param = param_find_no_notification(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown param: %s", name); _mavlink->send_statustext_info(buf); } else { // Load current value before setting it float curr_val; param_get(param, &curr_val); param_set(param, &(set.param_value)); // Check if the parameter changed. If it didn't change, send current value back if (!(fabsf(curr_val - set.param_value) > 0.0f)) { send_param(param); } } } if (set.target_system == mavlink_system.sysid && set.target_component < 127 && (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { // publish set request to UAVCAN driver via uORB. uavcan_parameter_request_s req; req.message_type = msg->msgid; req.node_id = set.target_component; req.param_index = -1; strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; if (set.param_type == MAV_PARAM_TYPE_REAL32) { req.param_type = MAV_PARAM_TYPE_REAL32; req.real_value = set.param_value; } else { int32_t val; memcpy(&val, &set.param_value, sizeof(int32_t)); req.param_type = MAV_PARAM_TYPE_INT64; req.int_value = val; } if (_uavcan_parameter_request_pub == nullptr) { _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); } else { orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); } } break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { /* request one parameter */ mavlink_param_request_read_t req_read; mavlink_msg_param_request_read_decode(msg, &req_read); if (req_read.target_system == mavlink_system.sysid && (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { /* XXX: I left this in so older versions of QGC wouldn't break */ if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) { /* return hash check for cached params */ uint32_t hash = param_hash_check(); /* build the one-off response message */ mavlink_param_value_t param_value; param_value.param_count = param_count_used(); param_value.param_index = -1; strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); param_value.param_type = MAV_PARAM_TYPE_UINT32; memcpy(¶m_value.param_value, &hash, sizeof(hash)); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_value); } else { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ send_param(param_find_no_notification(name)); } } else { /* when index is >= 0, send this parameter again */ int ret = send_param(param_for_used_index(req_read.param_index)); if (ret == 1) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); _mavlink->send_statustext_info(buf); } else if (ret == 2) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); _mavlink->send_statustext_info(buf); } } } if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { // publish set request to UAVCAN driver via uORB. uavcan_parameter_request_s req = {}; req.timestamp = hrt_absolute_time(); req.message_type = msg->msgid; req.node_id = req_read.target_component; req.param_index = req_read.param_index; strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; // Enque the request and forward the first to the uavcan node enque_uavcan_request(&req); request_next_uavcan_parameter(); } break; } case MAVLINK_MSG_ID_PARAM_MAP_RC: { /* map a rc channel to a parameter */ mavlink_param_map_rc_t map_rc; mavlink_msg_param_map_rc_decode(msg, &map_rc); if (map_rc.target_system == mavlink_system.sysid && (map_rc.target_component == mavlink_system.compid || map_rc.target_component == MAV_COMP_ID_ALL)) { /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ size_t i = map_rc.parameter_rc_channel_index; _rc_param_map.param_index[i] = map_rc.param_index; strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ _rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; _rc_param_map.value_min[i] = map_rc.param_value_min; _rc_param_map.value_max[i] = map_rc.param_value_max; if (map_rc.param_index == -2) { // -2 means unset map _rc_param_map.valid[i] = false; } else { _rc_param_map.valid[i] = true; } _rc_param_map.timestamp = hrt_absolute_time(); if (_rc_param_map_pub == nullptr) { _rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map); } else { orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map); } } break; } default: break; } }
void CcUnpackCycle(SerialDriver *sdp){ mavlink_message_t msg; mavlink_status_t status; msg_t c = 0; msg_t prev_c = 0; while (!chThdShouldTerminate()) { if (!cc_port_ready()){ chThdSleepMilliseconds(50); continue; } else{ /* Try to get an escaped with DLE symbols message */ c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50)); if (c == Q_TIMEOUT) continue; prev_c = c; if (prev_c == DLE){ prev_c = 0; /* set it to any just not DLE nor ETX */ c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50)); if (c == Q_TIMEOUT) continue; } } if (mavlink_parse_char(MAVLINK_COMM_0, (uint8_t)c, &msg, &status)) { switch(msg.msgid){ case MAVLINK_MSG_ID_COMMAND_LONG: mavlink_msg_command_long_decode(&msg, &mavlink_command_long_struct); if (mavlink_command_long_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_command_long, EVMSK_MAVLINK_COMMAND_LONG); break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: mavlink_msg_param_request_read_decode(&msg, &mavlink_param_request_read_struct); if (mavlink_param_request_read_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_param_request_read, EVMSK_MAVLINK_PARAM_REQUEST_READ); break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: mavlink_msg_param_request_list_decode(&msg, &mavlink_param_request_list_struct); if (mavlink_param_request_list_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_param_request_list, EVMSK_MAVLINK_PARAM_REQUEST_LIST); break; case MAVLINK_MSG_ID_PARAM_SET: mavlink_msg_param_set_decode(&msg, &mavlink_param_set_struct); if (mavlink_param_set_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_param_set, EVMSK_MAVLINK_PARAM_SET); break; case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_CC: mavlink_msg_oblique_storage_request_cc_decode(&msg, &mavlink_oblique_storage_request_cc_struct); if (mavlink_oblique_storage_request_cc_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_CC); break; case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_COUNT_CC: mavlink_msg_oblique_storage_request_count_cc_decode(&msg, &mavlink_oblique_storage_request_count_cc_struct); if (mavlink_oblique_storage_request_count_cc_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_count_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_CC); break; case MAVLINK_MSG_ID_HEARTBEAT_CC: mavlink_msg_heartbeat_cc_decode(&msg, &mavlink_heartbeat_cc_struct); // if (mavlink_heartbeat_cc_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_heartbeat_cc, EVMSK_MAVLINK_HEARTBEAT_CC); break; case MAVLINK_MSG_ID_STATUSTEXT: mavlink_msg_statustext_decode(&msg, &mavlink_statustext_struct); // if (mavlink_statustext_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_statustext, EVMSK_MAVLINK_STATUSTEXT); break; case MAVLINK_MSG_ID_OBLIQUE_AGPS: mavlink_msg_oblique_agps_decode(&msg, &mavlink_oblique_agps_struct); if (mavlink_oblique_agps_struct.target_system == mavlink_system_struct.sysid) chEvtBroadcastFlags(&event_mavlink_oblique_agps, EVMSK_MAVLINK_OBLIQUE_AGPS); break; default: break; } } } }
void MavlinkComm::_handleMessage(mavlink_message_t * msg) { uint32_t timeStamp = micros(); switch (msg->msgid) { _board->debug->printf_P(PSTR("message received: %d"), msg->msgid); case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t packet; mavlink_msg_heartbeat_decode(msg, &packet); _lastHeartBeat = micros(); break; } case MAVLINK_MSG_ID_GPS_RAW: { // decode mavlink_gps_raw_t packet; mavlink_msg_gps_raw_decode(msg, &packet); _navigator->setTimeStamp(timeStamp); _navigator->setLat(packet.lat * deg2Rad); _navigator->setLon(packet.lon * deg2Rad); _navigator->setAlt(packet.alt); _navigator->setYaw(packet.hdg * deg2Rad); _navigator->setGroundSpeed(packet.v); _navigator->setAirSpeed(packet.v); //_board->debug->printf_P(PSTR("received hil gps raw packet\n")); /* _board->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), packet.lat, packet.lon, packet.alt); */ break; } case MAVLINK_MSG_ID_HIL_STATE: { // decode mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); _navigator->setTimeStamp(timeStamp); _navigator->setRoll(packet.roll); _navigator->setPitch(packet.pitch); _navigator->setYaw(packet.yaw); _navigator->setRollRate(packet.rollspeed); _navigator->setPitchRate(packet.pitchspeed); _navigator->setYawRate(packet.yawspeed); _navigator->setVN(packet.vx/ 1e2); _navigator->setVE(packet.vy/ 1e2); _navigator->setVD(packet.vz/ 1e2); _navigator->setLat_degInt(packet.lat); _navigator->setLon_degInt(packet.lon); _navigator->setAlt(packet.alt / 1e3); _navigator->setXAccel(packet.xacc/ 1e3); _navigator->setYAccel(packet.xacc/ 1e3); _navigator->setZAccel(packet.xacc/ 1e3); break; } case MAVLINK_MSG_ID_ATTITUDE: { // decode mavlink_attitude_t packet; mavlink_msg_attitude_decode(msg, &packet); // set dcm hil sensor _navigator->setTimeStamp(timeStamp); _navigator->setRoll(packet.roll); _navigator->setPitch(packet.pitch); _navigator->setYaw(packet.yaw); _navigator->setRollRate(packet.rollspeed); _navigator->setPitchRate(packet.pitchspeed); _navigator->setYawRate(packet.yawspeed); //_board->debug->printf_P(PSTR("received hil attitude packet\n")); break; } case MAVLINK_MSG_ID_ACTION: { // decode mavlink_action_t packet; mavlink_msg_action_decode(msg, &packet); if (_checkTarget(packet.target, packet.target_component)) break; // do action sendText(SEVERITY_LOW, PSTR("action received")); switch (packet.action) { case MAV_ACTION_STORAGE_READ: AP_Var::load_all(); break; case MAV_ACTION_STORAGE_WRITE: AP_Var::save_all(); break; case MAV_ACTION_MOTORS_START: _controller->setMode(MAV_MODE_READY); break; case MAV_ACTION_CALIBRATE_GYRO: case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: _controller->setMode(MAV_MODE_LOCKED); _navigator->calibrate(); break; case MAV_ACTION_EMCY_KILL: case MAV_ACTION_CONFIRM_KILL: case MAV_ACTION_MOTORS_STOP: case MAV_ACTION_SHUTDOWN: _controller->setMode(MAV_MODE_LOCKED); break; case MAV_ACTION_LAUNCH: case MAV_ACTION_TAKEOFF: _guide->setMode(MAV_NAV_LIFTOFF); break; case MAV_ACTION_LAND: _guide->setCurrentIndex(0); _guide->setMode(MAV_NAV_LANDING); break; case MAV_ACTION_EMCY_LAND: _guide->setMode(MAV_NAV_LANDING); break; case MAV_ACTION_LOITER: case MAV_ACTION_HALT: _guide->setMode(MAV_NAV_LOITER); break; case MAV_ACTION_SET_AUTO: _controller->setMode(MAV_MODE_AUTO); break; case MAV_ACTION_SET_MANUAL: _controller->setMode(MAV_MODE_MANUAL); break; case MAV_ACTION_RETURN: _guide->setMode(MAV_NAV_RETURNING); break; case MAV_ACTION_NAVIGATE: case MAV_ACTION_CONTINUE: _guide->setMode(MAV_NAV_WAYPOINT); break; case MAV_ACTION_CALIBRATE_RC: case MAV_ACTION_REBOOT: case MAV_ACTION_REC_START: case MAV_ACTION_REC_PAUSE: case MAV_ACTION_REC_STOP: sendText(SEVERITY_LOW, PSTR("action not implemented")); break; default: sendText(SEVERITY_LOW, PSTR("unknown action")); break; } break; } case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { sendText(SEVERITY_LOW, PSTR("waypoint request list")); // decode mavlink_waypoint_request_list_t packet; mavlink_msg_waypoint_request_list_decode(msg, &packet); if (_checkTarget(packet.target_system, packet.target_component)) break; // Start sending waypoints mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid, _guide->getNumberOfCommands()); _cmdTimeLastSent = millis(); _cmdTimeLastReceived = millis(); _sendingCmds = true; _receivingCmds = false; _cmdDestSysId = msg->sysid; _cmdDestCompId = msg->compid; break; } case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { sendText(SEVERITY_LOW, PSTR("waypoint request")); // Check if sending waypiont if (!_sendingCmds) break; // decode mavlink_waypoint_request_t packet; mavlink_msg_waypoint_request_decode(msg, &packet); if (_checkTarget(packet.target_system, packet.target_component)) break; _board->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); AP_MavlinkCommand cmd(packet.seq); mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue, wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z); // update last waypoint comm stamp _cmdTimeLastSent = millis(); break; } case MAVLINK_MSG_ID_WAYPOINT_ACK: { sendText(SEVERITY_LOW, PSTR("waypoint ack")); // decode mavlink_waypoint_ack_t packet; mavlink_msg_waypoint_ack_decode(msg, &packet); if (_checkTarget(packet.target_system, packet.target_component)) break; // check for error //uint8_t type = packet.type; // ok (0), error(1) // turn off waypoint send _sendingCmds = false; break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { sendText(SEVERITY_LOW, PSTR("param request list")); // decode mavlink_param_request_list_t packet; mavlink_msg_param_request_list_decode(msg, &packet); if (_checkTarget(packet.target_system, packet.target_component)) break; // Start sending parameters - next call to ::update will kick the first one out _queuedParameter = AP_Var::first(); _queuedParameterIndex = 0; break; } case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { sendText(SEVERITY_LOW, PSTR("waypoint clear all")); // decode mavlink_waypoint_clear_all_t packet; mavlink_msg_waypoint_clear_all_decode(msg, &packet); if (_checkTarget(packet.target_system, packet.target_component)) break; // clear all waypoints uint8_t type = 0; // ok (0), error(1) _guide->setNumberOfCommands(1); _guide->setCurrentIndex(0); // send acknowledgement 3 times to makes sure it is received for (int i = 0; i < 3; i++) mavlink_msg_waypoint_ack_send(_channel, msg->sysid, msg->compid, type); break; } case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { sendText(SEVERITY_LOW, PSTR("waypoint set current")); // decode mavlink_waypoint_set_current_t packet; mavlink_msg_waypoint_set_current_decode(msg, &packet); Serial.print("Packet Sequence:"); Serial.println(packet.seq); if (_checkTarget(packet.target_system, packet.target_component)) break; // set current waypoint Serial.print("Current Index:"); Serial.println(_guide->getCurrentIndex()); Serial.flush(); _guide->setCurrentIndex(packet.seq); mavlink_msg_waypoint_current_send(_channel, _guide->getCurrentIndex()); break; } case MAVLINK_MSG_ID_WAYPOINT_COUNT: { sendText(SEVERITY_LOW, PSTR("waypoint count")); // decode mavlink_waypoint_count_t packet; mavlink_msg_waypoint_count_decode(msg, &packet); if (_checkTarget(packet.target_system, packet.target_component)) break; // start waypoint receiving if (packet.count > _cmdMax) { packet.count = _cmdMax; } _cmdNumberRequested = packet.count; _cmdTimeLastReceived = millis(); _receivingCmds = true; _sendingCmds = false; _cmdRequestIndex = 0; break; } case MAVLINK_MSG_ID_WAYPOINT: { sendText(SEVERITY_LOW, PSTR("waypoint")); // Check if receiving waypiont if (!_receivingCmds) { //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); break; } // decode mavlink_waypoint_t packet; mavlink_msg_waypoint_decode(msg, &packet); if (_checkTarget(packet.target_system, packet.target_component)) break; // check if this is the requested waypoint if (packet.seq != _cmdRequestIndex) { char warningMsg[50]; sprintf(warningMsg, "waypoint request out of sequence: (packet) %d / %d (ap)", packet.seq, _cmdRequestIndex); sendText(SEVERITY_HIGH, warningMsg); break; } _board->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), packet.x, packet.y, packet.z); // store waypoint AP_MavlinkCommand command(packet); //sendText(SEVERITY_HIGH, PSTR("waypoint stored")); _cmdRequestIndex++; if (_cmdRequestIndex == _cmdNumberRequested) { sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK); _receivingCmds = false; _guide->setNumberOfCommands(_cmdNumberRequested); // make sure curernt waypoint still exists if (_cmdNumberRequested > _guide->getCurrentIndex()) { _guide->setCurrentIndex(0); mavlink_msg_waypoint_current_send(_channel, _guide->getCurrentIndex()); } //sendText(SEVERITY_LOW, PSTR("waypoint ack sent")); } else if (_cmdRequestIndex > _cmdNumberRequested) { _receivingCmds = false; } _cmdTimeLastReceived = millis(); break; } case MAVLINK_MSG_ID_PARAM_SET: { sendText(SEVERITY_LOW, PSTR("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 (_checkTarget(packet.target_system, packet.target_component)) break; // set parameter char key[_paramNameLengthMax + 1]; strncpy(key, (char *) packet.param_id, _paramNameLengthMax); key[_paramNameLengthMax] = 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. const 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) { ((AP_Int32 *) vp)->set_and_save( packet.param_value + rounding_addition); } else if (var_type == AP_Var::k_typeid_int16) { ((AP_Int16 *) vp)->set_and_save( packet.param_value + rounding_addition); } else if (var_type == AP_Var::k_typeid_int8) { ((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(_channel, (int8_t *) key, vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is... } break; } // end case } }
void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_param_request_list_t req; mavlink_msg_param_request_list_decode(msg, &req); if (req.target_system == mavlink_system.sysid && (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); send_statustext_info("[pm] sending list"); } } break; case MAVLINK_MSG_ID_PARAM_SET: { /* Handle parameter setting */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ param_t param = param_find(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); send_statustext_info(buf); } else { /* set and send parameter */ param_set(param, &(mavlink_param_set.param_value)); mavlink_pm_send_param(param); } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ mavlink_pm_send_param_for_name(name); } else { /* when index is >= 0, send this parameter again */ mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); } } } break; } }
bool MavESP8266Component::handleMessage(MavESP8266Bridge* sender, mavlink_message_t* message) { // // TODO: These response messages need to be queued up and sent as part of the main loop and not all // at once from here. // //----------------------------------------------- //-- MAVLINK_MSG_ID_PARAM_SET if(message->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t param; mavlink_msg_param_set_decode(message, ¶m); DEBUG_LOG("MAVLINK_MSG_ID_PARAM_SET: %u %s\n", param.target_component, param.param_id); if(param.target_component == MAV_COMP_ID_UDP_BRIDGE) { _handleParamSet(sender, ¶m); return true; } //----------------------------------------------- //-- MAVLINK_MSG_ID_COMMAND_LONG } else if(message->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(message, &cmd); if(cmd.target_component == MAV_COMP_ID_ALL || cmd.target_component == MAV_COMP_ID_UDP_BRIDGE) { _handleCmdLong(sender, &cmd, cmd.target_component); //-- If it was directed to us, eat it and loop if(cmd.target_component == MAV_COMP_ID_UDP_BRIDGE) { return true; } } //----------------------------------------------- //-- MAVLINK_MSG_ID_PARAM_REQUEST_LIST } else if(message->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { mavlink_param_request_list_t param; mavlink_msg_param_request_list_decode(message, ¶m); DEBUG_LOG("MAVLINK_MSG_ID_PARAM_REQUEST_LIST: %u\n", param.target_component); if(param.target_component == MAV_COMP_ID_ALL || param.target_component == MAV_COMP_ID_UDP_BRIDGE) { _handleParamRequestList(sender); } //----------------------------------------------- //-- MAVLINK_MSG_ID_PARAM_REQUEST_READ } else if(message->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { mavlink_param_request_read_t param; mavlink_msg_param_request_read_decode(message, ¶m); //-- This component or all components? if(param.target_component == MAV_COMP_ID_ALL || param.target_component == MAV_COMP_ID_UDP_BRIDGE) { //-- If asking for hash, respond and pass through if(strncmp(param.param_id, kHASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) { _sendParameter(sender, kHASH_PARAM, getWorld()->getParameters()->paramHashCheck(), 0xFFFF); } else { _handleParamRequestRead(sender, ¶m); //-- If this was addressed to me only eat message if(param.target_component == MAV_COMP_ID_UDP_BRIDGE) { //-- Eat message (don't send it to FC) return true; } } } } //-- Couldn't handle the message, pass on return false; }