/**
\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<T>::handle_param_set(mavlink_message_t *msg)
{
Mav_Param *param;
enum var_type vtype;
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
param = Mav_Param::get_param_by_name(packet.param_id, vtype);
if (param != NULL)
{
param->set_value(packet.param_value, vtype);
#ifdef DEBUG_ALL
Serial.print(packet.param_id);Serial.print(",");
Serial.print(param->cast_to_float_mav(vtype));Serial.print(",");
Serial.println(vtype);
#endif
mavlink_msg_param_value_pack( _systemId,
MAV_COMP_ID_ALL,
msg,
packet.param_id,
param->cast_to_float_mav(vtype),
vtype,
_param_count,
Mav_Param::get_id_by_name(packet.param_id));
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
_serial.write(buf, len);
}
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
enum ap_var_type var_type;
// set parameter
AP_Param *vp;
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
// find existing param so we can get the old value
vp = AP_Param::find(key, &var_type);
if (vp == NULL) {
return;
}
float old_value = vp->cast_to_float(var_type);
// set the value
vp->set_float(packet.param_value, var_type);
/*
we force the save if the value is not equal to the old
value. This copes with the use of override values in
constructors, such as PID elements. Otherwise a set to the
default value which differs from the constructor value doesn't
save the change
*/
bool force_save = !is_equal(packet.param_value, old_value);
// save the change
vp->save(force_save);
// 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_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
if (DataFlash != NULL) {
DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
}
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
enum ap_var_type var_type;
// set parameter
AP_Param *vp;
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
// find existing param so we can get the old value
vp = AP_Param::find(key, &var_type);
if (vp == NULL) {
return;
}
float old_value = vp->cast_to_float(var_type);
// set the value
vp->set_float(packet.param_value, var_type);
/*
we force the save if the value is not equal to the old
value. This copes with the use of override values in
constructors, such as PID elements. Otherwise a set to the
default value which differs from the constructor value doesn't
save the change
*/
bool force_save = !is_equal(packet.param_value, old_value);
// save the change
vp->save(force_save);
if (DataFlash != NULL) {
DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
}
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
enum ap_var_type var_type;
// set parameter
AP_Param *vp;
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
vp = AP_Param::set_param_by_name(key, packet.param_value, &var_type);
if (vp == NULL) {
return;
}
// save the change
vp->save();
// 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_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
if (DataFlash != NULL) {
DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
}
}
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;
}
}
}
}
/**
* @brief Receive communication packets and handle them. Should be called at the system sample rate.
*
* This function decodes packets on the protocol level and also handles
* their value by calling the appropriate functions.
*/
void MavLinkReceive(void)
{
mavlink_message_t msg;
mavlink_status_t status;
// Track whether we actually handled any data in this function call.
// Used for updating the number of MAVLink messages handled
bool processedData = false;
while (GetLength(&uart1RxBuffer) > 0) {
processedData = true;
uint8_t c;
Read(&uart1RxBuffer, &c);
// Parse another byte and if there's a message found process it.
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
// Latch the groundstation system and component ID if we haven't yet.
if (!groundStationSystemId && !groundStationComponentId) {
groundStationSystemId = msg.sysid;
groundStationComponentId = msg.compid;
}
switch(msg.msgid) {
// If we are not doing any mission protocol operations, record the size of the incoming mission
// list and transition into the write missions state machine loop.
case MAVLINK_MSG_ID_MISSION_COUNT: {
uint8_t mavlinkNewMissionListSize = mavlink_msg_mission_count_get_count(&msg);
MavLinkEvaluateMissionState(MISSION_EVENT_COUNT_RECEIVED, &mavlinkNewMissionListSize);
} break;
// Handle receiving a mission.
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t currentMission;
mavlink_msg_mission_item_decode(&msg, ¤tMission);
MavLinkEvaluateMissionState(MISSION_EVENT_ITEM_RECEIVED, ¤tMission);
} break;
// Responding to a mission request entails moving into the first active state and scheduling a MISSION_COUNT message
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
MavLinkEvaluateMissionState(MISSION_EVENT_REQUEST_LIST_RECEIVED, NULL);
break;
// When a mission request message is received, respond with that mission information from the MissionManager
case MAVLINK_MSG_ID_MISSION_REQUEST: {
uint8_t receivedMissionIndex = mavlink_msg_mission_request_get_seq(&msg);
MavLinkEvaluateMissionState(MISSION_EVENT_REQUEST_RECEIVED, &receivedMissionIndex);
} break;
// Allow for clearing waypoints. Here we respond simply with an ACK message if we successfully
// cleared the mission list.
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
MavLinkEvaluateMissionState(MISSION_EVENT_CLEAR_ALL_RECEIVED, NULL);
break;
// Allow for the groundstation to set the current mission. This requires a WAYPOINT_CURRENT response message agreeing with the received current message index.
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
uint8_t newCurrentMission = mavlink_msg_mission_set_current_get_seq(&msg);
MavLinkEvaluateMissionState(MISSION_EVENT_SET_CURRENT_RECEIVED, &newCurrentMission);
} break;
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_msg_mission_ack_get_type(&msg);
MavLinkEvaluateMissionState(MISSION_EVENT_ACK_RECEIVED, NULL);
} break;
// If they're requesting a list of all parameters, call a separate function that'll track the state and transmit the necessary messages.
// This reason that this is an external function is so that it can be run separately at 20Hz.
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
MavLinkEvaluateParameterState(PARAM_EVENT_REQUEST_LIST_RECEIVED, NULL);
} break;
// If a request comes for a single parameter then set that to be the current parameter and move into the proper state.
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
uint16_t currentParameter = mavlink_msg_param_request_read_get_param_index(&msg);
MavLinkEvaluateParameterState(PARAM_EVENT_REQUEST_READ_RECEIVED, ¤tParameter);
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
mavlink_param_set_t x;
mavlink_msg_param_set_decode(&msg, &x);
MavLinkEvaluateParameterState(PARAM_EVENT_SET_RECEIVED, &x);
} break;
default: break;
}
}
}
// Update the number of messages received, both successful and not. Note that the 'status' variable
// will be updated on every call to *_parse_char(), so this will always be a valid value.
if (processedData) {
mavLinkMessagesReceived = status.packet_rx_success_count;
mavLinkMessagesFailedParsing = status.packet_rx_drop_count;
}
}
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_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg)
{
switch (msg->msgid)
{
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// Start sending parameters
pm.next_param = 0;
mavlink_missionlib_send_gcs_string("PM SENDING LIST");
}
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 (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid)
{
char* key = set.param_id;
for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++)
{
bool match = true;
for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++)
{
// Compare
if (pm.param_names[i][j] != key[j])
{
match = false;
}
// End matching if null termination is reached
if (pm.param_names[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 infinity
if (pm.param_values[i] != set.param_value
&& !isnan(set.param_value)
&& !isinf(set.param_value))
{
pm.param_values[i] = set.param_value;
// Report back new value
#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS
mavlink_message_t tx_msg;
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
MAVLINK_COMM_0,
&tx_msg,
pm.param_names[i],
pm.param_values[i],
MAVLINK_TYPE_FLOAT,
pm.size,
i);
mavlink_missionlib_send_message(&tx_msg);
#else
mavlink_msg_param_value_send(MAVLINK_COMM_0,
pm.param_names[i],
pm.param_values[i],
MAVLINK_TYPE_FLOAT,
pm.size,
i);
#endif
mavlink_missionlib_send_gcs_string("PM received param");
} // End valid value check
} // End match check
} // End for loop
} // End system ID check
} // End case
break;
} // End switch
}
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
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;
}
}
static void handle_message(mavlink_message_t *msg)
{
// Attitude MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT
// TODO update mavlink message to attitude yaw_rate
if (msg->msgid == MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT)
{
mavlink_roll_pitch_yaw_thrust_setpoint_t attitude_setpoint_mavlink_msg;
mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(msg, &attitude_setpoint_mavlink_msg);
laird_control_sp.mode = OFFBOARD_CONTROL_MODE_ATT_YAW_RATE;
laird_control_sp.p1 = attitude_setpoint_mavlink_msg.roll;
laird_control_sp.p2 = attitude_setpoint_mavlink_msg.pitch;
laird_control_sp.p3 = attitude_setpoint_mavlink_msg.yaw;
laird_control_sp.p4 = attitude_setpoint_mavlink_msg.thrust;
laird_control_sp.timestamp = hrt_absolute_time();
if (attitude_setpoint_mavlink_msg.thrust > 0)
{
laird_control_sp.armed = true;
}
else
{
laird_control_sp.armed = false;
}
/* check if topic has to be advertised */
if (laird_control_sp_pub <= 0)
{
laird_control_sp_pub = orb_advertise(ORB_ID(laird_control_setpoint), &laird_control_sp);
}
else
{
/* Publish */
orb_publish(ORB_ID(laird_control_setpoint), laird_control_sp_pub, &laird_control_sp);
}
} // end attitude
// Rate
if (msg->msgid == MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT)
{
mavlink_roll_pitch_yaw_speed_thrust_setpoint_t rate_setpoint_mavlink_msg;
mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(msg, &rate_setpoint_mavlink_msg);
laird_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
laird_control_sp.p1 = rate_setpoint_mavlink_msg.roll_speed;
laird_control_sp.p2 = rate_setpoint_mavlink_msg.pitch_speed;
laird_control_sp.p3 = rate_setpoint_mavlink_msg.yaw_speed;
laird_control_sp.p4 = rate_setpoint_mavlink_msg.thrust;
laird_control_sp.timestamp = hrt_absolute_time();
if (rate_setpoint_mavlink_msg.thrust > 0)
{
laird_control_sp.armed = true;
}
else
{
laird_control_sp.armed = false;
}
/* check if topic has to be advertised */
if (laird_control_sp_pub <= 0)
{
laird_control_sp_pub = orb_advertise(ORB_ID(laird_control_setpoint), &laird_control_sp);
}
else
{
/* Publish */
orb_publish(ORB_ID(laird_control_setpoint), laird_control_sp_pub, &laird_control_sp);
}
} // end rate
// parameter
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET)
{
mavlink_param_set_t param_mavlink_msg;
mavlink_msg_param_set_decode(msg, ¶m_mavlink_msg);
printf("%s %3.5f \n", param_mavlink_msg.param_id, param_mavlink_msg.param_value);
param_t param_ptr = param_find(param_mavlink_msg.param_id);
param_set(param_ptr, ¶m_mavlink_msg.param_value);
}
}
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;
}
}
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;
}
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */
global_data_parameter_storage->pm.next_param = 0;
mavlink_missionlib_send_gcs_string("[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))) {
uint16_t i; //parameters
uint16_t j; //chars
bool match;
for (i = 0; i < PARAM_MAX_COUNT; i++) {
match = true;
for (j = 0; j < MAX_PARAM_NAME_LEN; j++) {
/* Compare char by char */
if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_set.param_id[j]) {
match = false;
}
/* End matching if null termination is reached */
if (global_data_parameter_storage->pm.param_names[i][j] == '\0') {
break;
}
}
/* Check if matched */
if (match) {
// XXX handle param type as well, assuming float here
global_data_parameter_storage->pm.param_values[i] = mavlink_param_set.param_value;
mavlink_pm_send_one_parameter(i);
}
}
}
}
} 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) {
uint16_t i; //parameters
uint16_t j; //chars
bool match;
for (i = 0; i < PARAM_MAX_COUNT; i++) {
match = true;
for (j = 0; j < MAX_PARAM_NAME_LEN; j++) {
/* Compare char by char */
if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_request_read.param_id[j]) {
match = false;
}
/* End matching if null termination is reached */
if (global_data_parameter_storage->pm.param_names[i][j] == '\0') {
break;
}
}
/* Check if matched */
if (match) {
mavlink_pm_send_one_parameter(i);
}
}
} else {
/* when index is >= 0, send this parameter again */
mavlink_pm_send_one_parameter(mavlink_param_request_read.param_index);
}
}
} break;
}
}
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;
}
}
void onboard_parameters_receive_parameter(onboard_parameters_t* onboard_parameters, uint32_t sysid, mavlink_message_t* msg)
{
bool match = true;
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
// Check if this message is for this system and subsystem
if ( ((uint8_t)set.target_system == (uint8_t)sysid )
&& (set.target_component == onboard_parameters->mavlink_stream->compid) )
{
char* key = (char*) set.param_id;
onboard_parameters_entry_t* param;
if ( onboard_parameters->debug == true )
{
print_util_dbg_print("Setting parameter ");
print_util_dbg_print(set.param_id);
print_util_dbg_print(" to ");
print_util_dbg_putfloat(set.param_value, 2);
print_util_dbg_print("\r\n");
}
for (uint16_t i = 0; i < onboard_parameters->param_set->param_count; i++)
{
param = &onboard_parameters->param_set->parameters[i];
match = true;
for (uint16_t j = 0; j < param->param_name_length; j++)
{
// Compare
if ((char)param->param_name[j] != (char)key[j])
{
match = false;
}
// End matching if null termination is reached
if (((char)param->param_name[j]) == '\0')
{
// Exit internal (j) for() loop
break;
}
}
// Check if matched
if (match)
{
// Only write and emit changes if there is actually a difference
if (*(param->param) != set.param_value && set.param_type == param->data_type)
{
// onboard_parameters_update_parameter(onboard_parameters, i, set.param_value);
(*param->param) = set.param_value;
// schedule parameter for transmission downstream
param->schedule_for_transmission=true;
}
break;
}
}
if (!match)
{
if ( onboard_parameters->debug == true )
{
print_util_dbg_print("Set parameter error! Parameter ");
print_util_dbg_print(set.param_id);
print_util_dbg_print(" not registred!\r\n");
}
}
}
}
void handle_mav_link_mess(void)
{
int i,j;
mavlink_message_t msg;
mavlink_status_t status;
mavlink_param_set_t set;
char* key;
while(UART_CharAvailable())
{
uint8_t c = UART_GetChar();
if(mavlink_parse_char(0, c, &msg, &status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
param = 0;
break;
case MAVLINK_MSG_ID_PARAM_SET:
{
mavlink_msg_param_set_decode(&msg, &set);
key = (char*) set.param_id;
for (i = 0; i < ONBOARD_PARAM_COUNT; i++)
{
uint8_t match = 1;
for (j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
{
// Compare
if (((char) (global_data.param_name[i][j])) != (char) (key[j]))
{
match = 0;
}
// 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)
{
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, param);
}
}
}
}
break;
}
}
}
if (param < ONBOARD_PARAM_COUNT)
{
mavlink_msg_param_value_send(0,
(int8_t*) global_data.param_name[param],
global_data.param[param], ONBOARD_PARAM_COUNT, param);
param++;
}
}
void protDecodeMavlink(void) {
uint8_t indx, writeSuccess, commChannel = 1;
mavlink_param_set_t set;
mavlink_message_t msg;
mavlink_status_t status;
// uint8_t* dataIn;
// fix the data length so if the interrupt adds data
// during execution of this block, it will be read
// until the next gsRead
unsigned int tmpLen = getLength(uartMavlinkInBuffer), i = 0;
// if the buffer has more data than the max size, set it to max,
// otherwise set it to the length
//DatafromGSmavlink[0] = (tmpLen > MAXINLEN) ? MAXINLEN : tmpLen;
// read the data
//for (i = 1; i <= DatafromGSmavlink[0]; i += 1) {
//mavlink_parse_char(commChannel, readFront(uartBufferInMavlink), &msg, &status);
//DatafromGSmavlink[i] = readFront(uartMavlinkInBuffer);
//}
//dataIn = DatafromGSmavlink;
// increment the age of heartbeat
mlPending.heartbeatAge++;
for (i = 0; i <= tmpLen; i++) {
// Try to get a new message
if (mavlink_parse_char(commChannel, readFront(uartMavlinkInBuffer), &msg, &status)) {
// Handle message
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_msg_heartbeat_decode(&msg, &mlHeartbeat);
// Reset the heartbeat
mlPending.heartbeatAge = 0;
break;
//AM DBG
case MAVLINK_MSG_ID_MISSION_COUNT:
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
mavlink_msg_mission_count_decode(&msg, &mlWpCount);
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
// reset the rest of the state machine
mlPending.wpTotalWps = mlWpCount.count;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
// if there is no transaction going on
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_LIST_REQUESTED;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
mavlink_msg_mission_request_decode(&msg, &mlWpRequest);
if (mlPending.wpTransaction && (mlWpRequest.seq < mlWpValues.wpCount)) {
// change the state
mlPending.wpProtState = WP_PROT_TX_WP;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = mlWpRequest.seq;
mlPending.wpTimeOut = 0;
} else {
// TODO: put here a report for a single WP, i.e. not inside a transaction
}
break;
case MAVLINK_MSG_ID_MISSION_ACK:
mavlink_msg_mission_ack_decode(&msg, &mlWpAck);
if (mlPending.wpTransaction) {
// End the transaction
mlPending.wpTransaction = 0;
// change the state
mlPending.wpProtState = WP_PROT_IDLE;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
// send current waypoint index
mlPending.wpSendCurrent = TRUE;
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
writeSuccess = SUCCESS;
mavlink_msg_mission_item_decode(&msg, &mlSingleWp);
if (mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_RX_WP)) {
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
}
indx = (uint8_t) mlSingleWp.seq;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = mlSingleWp.command;
mlWpValues.orbit[indx] = (uint16_t) mlSingleWp.param3;
/*
// Record the data in EEPROM
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
// Set the flag of Aknowledge for the AKN Message
// if the write was not successful
if (writeSuccess != SUCCESS) {
mlPending.wpAck++;
mlWpAck.target_component = MAV_COMP_ID_MISSIONPLANNER;
mlWpAck.type = MAV_MISSION_ERROR;
}
*/
break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
writeSuccess = SUCCESS;
// clear the WP values in memory;
memset(&mlWpValues, 0, sizeof (mavlink_mission_item_values_t));
/*
writeSuccess = clearWaypointsFrom(0);
// Set the flag of Aknowledge fail
// if the write was unsuccessful
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to clear waypoints from EEPROM.", 49);
}
*/
// Update the waypoint count
mlWpValues.wpCount = 0;
// Set the state machine ready to send the WP akn
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTotalWps = 0;
mlPending.wpTransaction = 1;
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
break;
case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
writeSuccess = SUCCESS;
memset(&mlSingleWp, 0, sizeof (mavlink_mission_item_t));
mavlink_msg_set_gps_global_origin_decode(&msg, &mlGSLocation);
mlSingleWp.x = (float) (mlGSLocation.latitude);
mlSingleWp.y = (float) (mlGSLocation.longitude);
mlSingleWp.z = (float) (mlGSLocation.altitude);
indx = (uint8_t) MAX_NUM_WPS - 1;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = MAV_CMD_NAV_LAND;
mlWpValues.orbit[indx] = 0;
// Record the data in EEPROM
/*
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to write origin to EEPROM.", 49);
}
else {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_INFO;
strncpy(mlStatustext.text, "Control DSC GPS origin set.", 49);
}
*/
break;
//AM DBG
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
mlPending.piTransaction = 1;
mlPending.piProtState = PI_SEND_ALL_PARAM;
mlPending.piCurrentParamInTransaction = 0;
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
// If it was in the middle of a list transmission or there is already a param enqueued
mlPending.piTransaction = 1;
switch (mlPending.piProtState) {
case PI_IDLE:
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = mavlink_msg_param_request_read_get_param_index(&msg); // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ALL_PARAM:
mlPending.piBackToList = 1; // mark to go back
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ONE_PARAM:
if (mlPending.piBackToList) {
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
}
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
mavlink_msg_param_set_decode(&msg, &set);
if ((uint8_t) set.target_system == (uint8_t) SYSTEMID &&
(uint8_t) set.target_component == (uint8_t) COMPID) {
char* key = (char*) set.param_id;
uint8_t i, j;
uint8_t match;
for (i = 0; i < PAR_PARAM_COUNT; i++) {
match = 1;
for (j = 0; j < PARAM_NAME_LENGTH; j++) {
// Compare
if (((char) (mlParamInterface.param_name[i][j]))
!= (char) (key[j])) {
match = 0;
} // if
// End matching if null termination is reached
if (((char) mlParamInterface.param_name[i][j]) == '\0') {
break;
} // if
}// for j
// Check if matched
if (match) {
//sw_debug = 1;
// 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 infinity
if (isFinite(set.param_value)) {
mlParamInterface.param[i] = set.param_value;
// Report back new value
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = i; // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
mlPending.piTransaction = 1;
} // if different and not nan and not inf
} // if match
}// for i
} // if addressed to this
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 mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink 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, "[mavlink pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(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;
case MAVLINK_MSG_ID_COMMAND_LONG: {
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
uint8_t result = MAV_RESULT_UNSUPPORTED;
if (cmd_mavlink.target_system == mavlink_system.sysid &&
((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
/* preflight parameter load / store */
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
/* Read all parameters from EEPROM to RAM */
if (((int)(cmd_mavlink.param1)) == 0) {
/* read all parameters from EEPROM to RAM */
int read_ret = mavlink_pm_load_eeprom();
if (read_ret == OK) {
//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params");
result = MAV_RESULT_ACCEPTED;
} else if (read_ret == 1) {
mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load");
result = MAV_RESULT_ACCEPTED;
} else {
if (read_ret < -1) {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM");
} else {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found");
}
result = MAV_RESULT_FAILED;
}
} else if (((int)(cmd_mavlink.param1)) == 1) {
/* write all parameters from RAM to EEPROM */
int write_ret = mavlink_pm_save_eeprom();
if (write_ret == OK) {
mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM");
result = MAV_RESULT_ACCEPTED;
} else {
if (write_ret < -1) {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM");
} else {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found");
}
result = MAV_RESULT_FAILED;
}
} else {
//fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request");
result = MAV_RESULT_UNSUPPORTED;
}
}
}
/* send back command result */
//mavlink_msg_command_ack_send(chan, cmd.command, result);
} break;
}
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
AP_Param *vp;
enum ap_var_type var_type;
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
return;
}
// set parameter
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
// find the requested parameter
vp = AP_Param::find(key, &var_type);
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;
// handle variables with standard type IDs
if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -2147483648.0, 2147483647.0);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -128, 127);
((AP_Int8 *)vp)->set_and_save(v);
} else {
// we don't support mavlink set on this parameter
return;
}
// 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_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
if (DataFlash != NULL) {
DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
}
}
}
void handle_mavlink_message(mavlink_channel_t chan,
mavlink_message_t* msg)
{
/* all messages from usart2 are directly forwarded */
if(chan == MAVLINK_COMM_1)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint32_t len;
// Copy to USART 3
len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(MAVLINK_COMM_0, buf, len);
if(global_data.param[PARAM_USB_SEND_FORWARD])
mavlink_send_uart_bytes(MAVLINK_COMM_2, buf, len);
return;
}
/* forwarded received messages from usb and usart3 to usart2 */
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint32_t len;
/* copy to usart2 */
len = mavlink_msg_to_send_buffer(buf, msg);
for (int i = 0; i < len; i++)
{
usart2_tx_ringbuffer_push(buf, len);
}
/* handling messages */
switch (msg->msgid)
{
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,
global_data.param_name[set.param_index],
global_data.param[set.param_index], MAVLINK_TYPE_FLOAT, 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,
global_data.param_name[i],
global_data.param[i], MAVLINK_TYPE_FLOAT, 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 infinity
*/
if (global_data.param[i] != set.param_value
&& !isnan(set.param_value)
&& !isinf(set.param_value)
&& global_data.param_access[i])
{
global_data.param[i] = set.param_value;
/* handle sensor position */
if(i == PARAM_SENSOR_POSITION)
{
set_sensor_position_settings((uint8_t) set.param_value);
mt9v034_context_configuration();
dma_reconfigure();
buffer_reset();
}
/* handle low light mode and noise correction */
else if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR)
{
mt9v034_context_configuration();
dma_reconfigure();
buffer_reset();
}
/* handle calibration on/off */
else if(i == PARAM_CALIBRATION_ON)
{
mt9v034_set_context();
dma_reconfigure();
buffer_reset();
if(global_data.param[PARAM_CALIBRATION_ON])
debug_string_message_buffer("Calibration Mode On");
else
debug_string_message_buffer("Calibration Mode Off");
}
/* handle sensor position */
else if(i == PARAM_GYRO_SENSITIVITY_DPS)
{
l3gd20_config();
}
else
{
debug_int_message_buffer("Parameter received, param id =", i);
}
/* report back new value */
mavlink_msg_param_value_send(MAVLINK_COMM_0,
global_data.param_name[i],
global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i);
mavlink_msg_param_value_send(MAVLINK_COMM_2,
global_data.param_name[i],
global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i);
}
else
{
/* send back current value because it is not accepted or not write access*/
mavlink_msg_param_value_send(MAVLINK_COMM_0,
global_data.param_name[i],
global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i);
mavlink_msg_param_value_send(MAVLINK_COMM_2,
global_data.param_name[i],
global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i);
}
}
}
}
}
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 = get_boot_time_ms() * 1000;
mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp);
}
}
break;
default:
break;
}
}
//@{
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 cDataLink::handleMessage(mavlink_message_t* msg)
{
uint16_t len;
int bytes_sent;
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
//uint8_t result = MAV_RESULT_UNSUPPORTED;
switch (msg->msgid)
{
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
switch(packet.command)
{
case MAV_CMD_PREFLIGHT_CALIBRATION:
// 1: Gyro calibration
if (packet.param2 == 1)
{
printf("Magnetometer calibration!\n");
signal_mag_calibration();
}
// 3: Ground pressure
// 4: Radio calibration
// 5: Accelerometer calibration
// Parameter 6 & 7 not pre-defined
// result = MAV_RESULT_ACCEPTED;
break;
default:
// result = MAV_RESULT_UNSUPPORTED;
break;
} // switch packet.command
// mavlink_msg_command_ack_send(chan,
// packet.command,
// result);
break;
} // case MAVLINK_MSG_ID_COMMAND_LONG
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
mavlink_param_request_read_t packet;
mavlink_msg_param_request_read_decode(msg, &packet);
mavlink_msg_param_value_pack(
packet.target_system,
packet.target_component,
msg,
cParameter::get_instances()->at(packet.param_index)->get_name().c_str(),
*(cParameter::get_instances()->at(packet.param_index)->get_value()),
MAV_PARAM_TYPE_REAL32,
cParameter::get_instances()->size(), // param_count = Anzahl der Parameter
packet.param_index); // param_index = Nummer f. aktellen Parameter
len = mavlink_msg_to_send_buffer(m_buf, msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// Start sending parameters
for(std::vector<cParameter*>::iterator it=cParameter::get_instances()->begin(); it!=cParameter::get_instances()->end(); ++it)
{
mavlink_msg_param_value_pack(
1, MAV_COMP_ID_SYSTEM_CONTROL,
msg,
(*it)->get_name().c_str(),
*((*it)->get_value()), //Iterator goes to vector list with cParameter pointers. Get value returns a pointer to the component value -> again dereference
MAV_PARAM_TYPE_REAL32,
cParameter::get_instances()->size(), // param_count = Anzahl der Parameter
std::distance(cParameter::get_instances()->begin(),it)); // param_index = Nummer f. aktellen Parameter
len = mavlink_msg_to_send_buffer(m_buf, msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
boost::this_thread::sleep(boost::posix_time::milliseconds(10));
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
std::vector<cParameter*>::iterator it = cParameter::exist(packet.param_id);
if( cParameter::get_instances()->end() != it )
{
(*it)->set_value(packet.param_value);
mavlink_msg_param_value_pack(
packet.target_system,
packet.target_component,
msg,
(*it)->get_name().c_str(),
*((*it)->get_value()),
MAV_PARAM_TYPE_REAL32,
cParameter::get_instances()->size(), // param_count = Anzahl der Parameter
std::distance(cParameter::get_instances()->begin(),it)); // param_index = Nummer f. aktellen Parameter
len = mavlink_msg_to_send_buffer(m_buf, msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
break;
}
default:
{
printf("msg->msgid not known: %d\n", msg->msgid);
}
break;
}// switch msgid
}
