/**
* @brief Send one low-priority parameter message
*/
void communication_parameter_send(void)
{
/* send parameters one by one */
if (m_parameter_i < ONBOARD_PARAM_COUNT)
{
mavlink_msg_param_value_send(MAVLINK_COMM_0,
global_data.param_name[m_parameter_i],
global_data.param[m_parameter_i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i);
mavlink_msg_param_value_send(MAVLINK_COMM_2,
global_data.param_name[m_parameter_i],
global_data.param[m_parameter_i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i);
m_parameter_i++;
}
}
/**
* @brief Send low-priority messages at a maximum rate of xx Hertz
*
* This function sends messages at a lower rate to not exceed the wireless
* bandwidth. It sends one message each time it is called until the buffer is empty.
* Call this function with xx Hertz to increase/decrease the bandwidth.
*/
void communication_queued_send(void)
{
//send parameters one by one
// for (int m_parameter_i = 0; m_parameter_i < ONBOARD_PARAM_COUNT; i++)
if (m_parameter_i < ONBOARD_PARAM_COUNT)
{
mavlink_msg_param_value_send(MAVLINK_COMM_0,
(int8_t*) global_data.param_name[m_parameter_i],
global_data.param[m_parameter_i], ONBOARD_PARAM_COUNT, m_parameter_i);
mavlink_msg_param_value_send(MAVLINK_COMM_1,
(int8_t*) global_data.param_name[m_parameter_i],
global_data.param[m_parameter_i], ONBOARD_PARAM_COUNT, m_parameter_i);
m_parameter_i++;
}
}
/**
* sends parameters one at a time
*/
void MavlinkComm::sendParameters() {
//_board->debug->printf_P(PSTR("send parameters\n"));
// Check to see if we are sending parameters
while (NULL != _queuedParameter) {
AP_Var *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queuedParameter;
_queuedParameter = _queuedParameter->next();
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float();
if (!isnan(value)) {
char paramName[_paramNameLengthMax];
vp->copy_name(paramName, sizeof(paramName));
mavlink_msg_param_value_send(_channel, (int8_t*) paramName,
value, _countParameters(), _queuedParameterIndex);
_queuedParameterIndex++;
break;
}
}
}
void handle_message(mavlink_channel_t chan, mavlink_message_t* msg) {
hal.console->printf_P(PSTR("SimpleGCS Handle Message %d\r\n"), msg->msgid);
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
char param_name[16] = "NOPARAMS";
/* Send a single parameter.*/
mavlink_msg_param_value_send(
chan,
param_name,
0.0f, /* param value */
MAVLINK_TYPE_FLOAT, /* param type */
1, /* _queued_parameter_count */
0); /* _queued_parameter_index */
break;
}
case MAVLINK_MSG_ID_DATA16:
gcs_console_handle_data16(msg);
break;
case MAVLINK_MSG_ID_DATA32:
gcs_console_handle_data32(msg);
break;
}
}
/**
* @brief Send low-priority messages at a maximum rate of xx Hertz
*
* This function sends messages at a lower rate to not exceed the wireless
* bandwidth. It sends one message each time it is called until the buffer is empty.
* Call this function with xx Hertz to increase/decrease the bandwidth.
*/
void mavlink_pm_queued_send(void)
{
//send parameters one by one
if (pm.next_param < pm.size)
{
//for (int i.. all active comm links)
#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[pm.next_param],
pm.param_values[pm.next_param],
MAVLINK_TYPE_FLOAT,
pm.size,
pm.next_param);
mavlink_missionlib_send_message(&tx_msg);
#else
mavlink_msg_param_value_send(MAVLINK_COMM_0,
pm.param_names[pm.next_param],
pm.param_values[pm.next_param],
MAV_DATA_TYPE_FLOAT,
pm.size,
pm.next_param);
#endif
pm.next_param++;
}
}
/**
* @brief Send the next pending parameter, called from deferred message
* handling code
*/
void
GCS_MAVLINK::queued_param_send()
{
if (!initialised || _queued_parameter == NULL) {
return;
}
uint16_t bytes_allowed;
uint8_t count;
uint32_t tnow = hal.scheduler->millis();
// use at most 30% of bandwidth on parameters. The constant 26 is
// 1/(1000 * 1/8 * 0.001 * 0.3)
bytes_allowed = 57 * (tnow - _queued_parameter_send_time_ms) * 26;
if (bytes_allowed > comm_get_txspace(chan)) {
bytes_allowed = comm_get_txspace(chan);
}
count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
// when we don't have flow control we really need to keep the
// param download very slow, or it tends to stall
if (!have_flow_control() && count > 5) {
count = 5;
}
while (_queued_parameter != NULL && count--) {
AP_Param *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float(_queued_parameter_type);
char param_name[AP_MAX_NAME_SIZE];
vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
mavlink_msg_param_value_send(
chan,
param_name,
value,
mav_var_type(_queued_parameter_type),
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index++;
}
_queued_parameter_send_time_ms = tnow;
}
/**
* Send the parameters
*/
static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev)
{
if (mavlink_params_idx >= NB_SETTING) {
return;
}
mavlink_msg_param_value_send(MAVLINK_COMM_0,
mavlink_param_names[mavlink_params_idx],
settings_get_value(mavlink_params_idx),
MAV_PARAM_TYPE_REAL32,
NB_SETTING,
mavlink_params_idx);
MAVLinkSendMessage();
mavlink_params_idx++;
}
/**
* Send the parameters
*/
static inline void mavlink_send_params(void)
{
if (mavlink_params_idx >= NB_SETTING) {
return;
}
mavlink_msg_param_value_send(MAVLINK_COMM_0,
mavlink_param_names[mavlink_params_idx],
settings_get_value(mavlink_params_idx),
MAV_PARAM_TYPE_REAL32,
NB_SETTING,
mavlink_params_idx);
MAVLinkSendMessage();
mavlink_params_idx++;
}
/*
send a parameter value message to all active MAVLink connections
*/
void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
mavlink_msg_param_value_send(
chan,
param_name,
param_value,
mav_var_type(param_type),
AP_Param::count_parameters(),
-1);
}
}
}
}
//@{
void handle_mavlink_message(mavlink_channel_t chan,
mavlink_message_t* msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint32_t len;
switch (chan)
{
case MAVLINK_COMM_0:
{
if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE)
{
// Copy to COMM 1
len = mavlink_msg_to_send_buffer(buf, msg);
for (int i = 0; i < len; i++)
{
uart1_transmit(buf[i]);
}
}
}
break;
case MAVLINK_COMM_1:
{
if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE && msg->msgid != MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE)
{
// Copy to COMM 0
len = mavlink_msg_to_send_buffer(buf, msg);
for (int i = 0; i < len; i++)
{
uart0_transmit(buf[i]);
}
break;
}
}
default:
break;
}
switch (msg->msgid)
{
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(msg, &mode);
// Check if this system should change the mode
if (mode.target == (uint8_t)global_data.param[PARAM_SYSTEM_ID])
{
sys_set_mode(mode.mode);
// Emit current mode
mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
}
}
break;
case MAVLINK_MSG_ID_ACTION:
{
execute_action(mavlink_msg_action_get_action(msg));
//Forwart actions from Xbee to Onboard Computer and vice versa
if (chan == MAVLINK_COMM_1)
{
mavlink_send_uart(MAVLINK_COMM_0, msg);
}
else if (chan == MAVLINK_COMM_0)
{
mavlink_send_uart(MAVLINK_COMM_1, msg);
}
}
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
{
if (!sys_time_clock_get_unix_offset())
{
int64_t offset = ((int64_t) mavlink_msg_system_time_get_time_usec(
msg)) - (int64_t) sys_time_clock_get_time_usec();
sys_time_clock_set_unix_offset(offset);
debug_message_buffer("UNIX offset updated");
}
else
{
// debug_message_buffer("UNIX offset REFUSED");
}
}
break;
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
mavlink_request_data_stream_t stream;
mavlink_msg_request_data_stream_decode(msg, &stream);
switch (stream.req_stream_id)
{
case 0: // UNIMPLEMENTED
break;
case 1: // RAW SENSOR DATA
global_data.param[PARAM_SEND_SLOT_RAW_IMU] = stream.start_stop;
break;
case 2: // EXTENDED SYSTEM STATUS
global_data.param[PARAM_SEND_SLOT_ATTITUDE] = stream.start_stop;
break;
case 3: // REMOTE CONTROL CHANNELS
global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] = stream.start_stop;
break;
case 4: // RAW CONTROLLER
//global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
//global_data.param[PARAM_SEND_SLOT_DEBUG_3] = stream.start_stop;
global_data.param[PARAM_SEND_SLOT_CONTROLLER_OUTPUT] = stream.start_stop;
break;
case 5: // SENSOR FUSION
//LOST IN GROUDNCONTROL
// global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
break;
case 6: // POSITION
global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
break;
case 7: // EXTRA1
global_data.param[PARAM_SEND_SLOT_DEBUG_2] = stream.start_stop;
break;
case 8: // EXTRA2
global_data.param[PARAM_SEND_SLOT_DEBUG_4] = stream.start_stop;
break;
case 9: // EXTRA3
global_data.param[PARAM_SEND_SLOT_DEBUG_6] = stream.start_stop;
break;
default:
// Do nothing
break;
}
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
mavlink_param_request_read_t set;
mavlink_msg_param_request_read_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system
== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
&& (uint8_t) set.target_component
== (uint8_t) global_data.param[PARAM_COMPONENT_ID])
{
char* key = (char*) set.param_id;
if (set.param_id[0] == '\0')
{
// Choose parameter based on index
if (set.param_index < ONBOARD_PARAM_COUNT)
{
// Report back value
mavlink_msg_param_value_send(chan,
(int8_t*) global_data.param_name[set.param_index],
global_data.param[set.param_index], ONBOARD_PARAM_COUNT, set.param_index);
}
}
else
{
for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
{
bool match = true;
for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
{
// Compare
if (((char) (global_data.param_name[i][j]))
!= (char) (key[j]))
{
match = false;
}
// End matching if null termination is reached
if (((char) global_data.param_name[i][j]) == '\0')
{
break;
}
}
// Check if matched
if (match)
{
// Report back value
mavlink_msg_param_value_send(chan,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
}
}
}
}
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// Start sending parameters
m_parameter_i = 0;
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
{
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system
== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
&& (uint8_t) set.target_component
== (uint8_t) global_data.param[PARAM_COMPONENT_ID])
{
char* key = (char*) set.param_id;
for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
{
bool match = true;
for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
{
// Compare
if (((char) (global_data.param_name[i][j]))
!= (char) (key[j]))
{
match = false;
}
// End matching if null termination is reached
if (((char) global_data.param_name[i][j]) == '\0')
{
break;
}
}
// Check if matched
if (match)
{
// Only write and emit changes if there is actually a difference
// AND only write if new value is NOT "not-a-number"
// AND is NOT infy
if (global_data.param[i] != set.param_value
&& !isnan(set.param_value)
&& !isinf(set.param_value))
{
global_data.param[i] = set.param_value;
// Report back new value
mavlink_msg_param_value_send(MAVLINK_COMM_0,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
mavlink_msg_param_value_send(MAVLINK_COMM_1,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
debug_message_buffer_sprintf("Parameter received param id=%i",i);
}
}
}
}
}
break;
case MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET:
{
mavlink_position_control_setpoint_set_t pos;
mavlink_msg_position_control_setpoint_set_decode(msg, &pos);
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
{
// global_data.position_setpoint.x = pos.x;
// global_data.position_setpoint.y = pos.y;
// global_data.position_setpoint.z = pos.z;
debug_message_buffer("Position setpoint updated. OLD?\n");
}
else
{
debug_message_buffer(
"Position setpoint recieved but not updated. OLD?\n");
}
// Send back a message confirming the new position
mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_0, pos.id,
pos.x, pos.y, pos.z, pos.yaw);
mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1, pos.id,
pos.x, pos.y, pos.z, pos.yaw);
}
break;
case MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET:
{
mavlink_position_control_offset_set_t set;
mavlink_msg_position_control_offset_set_decode(msg, &set);
//global_data.attitude_setpoint_pos_body_offset.z = set.yaw;
//Ball Tracking
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1 && global_data.param[PARAM_POSITION_YAW_TRACKING]==1)
{
global_data.param[PARAM_POSITION_SETPOINT_YAW]
= global_data.attitude.z + set.yaw;
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 92, set.yaw);
}
}
break;
case MAVLINK_MSG_ID_SET_CAM_SHUTTER:
{
// Decode the desired shutter
mavlink_set_cam_shutter_t cam;
mavlink_msg_set_cam_shutter_decode(msg, &cam);
shutter_set(cam.interval, cam.exposure);
debug_message_buffer_sprintf("set_cam_shutter. interval %i",
cam.interval);
}
break;
case MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL:
{
uint8_t enable = mavlink_msg_image_trigger_control_get_enable(msg);
shutter_control(enable);
if (enable)
{
debug_message_buffer("CAM: Enabling hardware trigger");
}
else
{
debug_message_buffer("CAM: Disabling hardware trigger");
}
}
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
{
mavlink_vision_position_estimate_t pos;
mavlink_msg_vision_position_estimate_decode(msg, &pos);
vision_buffer_handle_data(&pos);
// Update validity time is done in vision buffer
}
break;
case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
{
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
global_data.vicon_data.x = pos.x;
global_data.vicon_data.y = pos.y;
global_data.vicon_data.z = pos.z;
global_data.state.vicon_new_data=1;
// Update validity time
global_data.vicon_last_valid = sys_time_clock_get_time_usec();
global_data.state.vicon_ok=1;
// //Set data from Vicon into vision filter
// global_data.vision_data.ang.x = pos.roll;
// global_data.vision_data.ang.y = pos.pitch;
// global_data.vision_data.ang.z = pos.yaw;
//
// global_data.vision_data.pos.x = pos.x;
// global_data.vision_data.pos.y = pos.y;
// global_data.vision_data.pos.z = pos.z;
//
// global_data.vision_data.new_data = 1;
if (!global_data.state.vision_ok)
{
//Correct YAW
global_data.attitude.z = pos.yaw;
//If yaw goes to infy (no idea why) set it to setpoint, next time will be better
if (global_data.attitude.z > 18.8495559 || global_data.attitude.z
< -18.8495559)
{
global_data.attitude.z = global_data.yaw_pos_setpoint;
debug_message_buffer(
"vicon CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
}
}
//send the vicon message to UART0 with new timestamp
mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_0, global_data.vicon_last_valid, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
}
break;
case MAVLINK_MSG_ID_PING:
{
mavlink_ping_t ping;
mavlink_msg_ping_decode(msg, &ping);
if (ping.target_system == 0 && ping.target_component == 0)
{
// Respond to ping
uint64_t r_timestamp = sys_time_clock_get_unix_time();
mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp);
}
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
{
mavlink_local_position_setpoint_set_t sp;
mavlink_msg_local_position_setpoint_set_decode(msg, &sp);
if (sp.target_system == global_data.param[PARAM_SYSTEM_ID])
{
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
{
if (sp.x >= global_data.position_setpoint_min.x && sp.y
>= global_data.position_setpoint_min.y && sp.z
>= global_data.position_setpoint_min.z && sp.x
<= global_data.position_setpoint_max.x && sp.y
<= global_data.position_setpoint_max.y && sp.z
<= global_data.position_setpoint_max.z)
{
debug_message_buffer("Setpoint accepted and set.");
global_data.param[PARAM_POSITION_SETPOINT_X] = sp.x;
global_data.param[PARAM_POSITION_SETPOINT_Y] = sp.y;
global_data.param[PARAM_POSITION_SETPOINT_Z] = sp.z;
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 0)
{
// Only update yaw if we are not tracking ball.
global_data.param[PARAM_POSITION_SETPOINT_YAW] = sp.yaw;
}
//check if we want to start or land
if (global_data.state.status == MAV_STATE_ACTIVE
|| global_data.state.status == MAV_STATE_CRITICAL)
{
if (sp.z > -0.1)
{
if (!(global_data.state.fly == FLY_GROUNDED
|| global_data.state.fly == FLY_SINKING
|| global_data.state.fly
== FLY_WAIT_LANDING
|| global_data.state.fly == FLY_LANDING
|| global_data.state.fly == FLY_RAMP_DOWN))
{
//if setpoint is lower that ground iate landing
global_data.state.fly = FLY_SINKING;
global_data.param[PARAM_POSITION_SETPOINT_Z]
= -0.2;//with lowpass
debug_message_buffer(
"Sinking for LANDING. (z-sp lower than 10cm)");
}
else if (!(global_data.state.fly == FLY_GROUNDED))
{
global_data.param[PARAM_POSITION_SETPOINT_Z]
= -0.2;//with lowpass
}
}
else if (global_data.state.fly == FLY_GROUNDED && sp.z
< -0.50)
{
//start if we were grounded and get a sp over 0.5m
global_data.state.fly = FLY_WAIT_MOTORS;
debug_message_buffer(
"STARTING wait motors. (z-sp higher than 50cm)");
//set point changed with lowpass, after 5s it will be ok.
}
}
//SINK TO 0.7m if we are critical or emergency
if (global_data.state.status == MAV_STATE_EMERGENCY
|| global_data.state.status == MAV_STATE_CRITICAL)
{
global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.7;//with lowpass
}
}
else
{
debug_message_buffer("Setpoint refused. Out of range.");
}
}
else
{
debug_message_buffer("Setpoint refused. Param setpoint accept=0.");
}
}
}
break;
default:
break;
}
}
void mavlink_common_message_handler(const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
break;
/* When requesting data streams say we can't send them */
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
mavlink_request_data_stream_t cmd;
mavlink_msg_request_data_stream_decode(msg, &cmd);
mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
MAVLinkSendMessage();
break;
}
/* Override channels with RC */
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
mavlink_rc_channels_override_t cmd;
mavlink_msg_rc_channels_override_decode(msg, &cmd);
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw);
//printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n",
// cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw);
#endif
break;
}
/* When a request is made of the parameters list */
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_params_idx = 0;
break;
}
/* When a request os made for a specific parameter */
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_param_request_read_t cmd;
mavlink_msg_param_request_read_decode(msg, &cmd);
// First check param_index and search for the ID if needed
if (cmd.param_index == -1) {
cmd.param_index = settings_idx_from_param_id(cmd.param_id);
}
// Send message only if the param_index was found (Coverity Scan)
if (cmd.param_index > -1) {
mavlink_msg_param_value_send(MAVLINK_COMM_0,
mavlink_param_names[cmd.param_index],
settings_get_value(cmd.param_index),
MAV_PARAM_TYPE_REAL32,
NB_SETTING,
cmd.param_index);
MAVLinkSendMessage();
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system == AC_ID) {
int16_t idx = settings_idx_from_param_id(set.param_id);
// setting found
if (idx >= 0) {
// Only write if new value is NOT "not-a-number"
// AND is NOT infinity
if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
!isnan(set.param_value) && !isinf(set.param_value)) {
DlSetting(idx, set.param_value);
// Report back new value
mavlink_msg_param_value_send(MAVLINK_COMM_0,
mavlink_param_names[idx],
settings_get_value(idx),
MAV_PARAM_TYPE_REAL32,
NB_SETTING,
idx);
MAVLinkSendMessage();
}
}
}
}
break;
#ifndef AP
/* only for rotorcraft */
case MAVLINK_MSG_ID_COMMAND_LONG: {
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
// Check if this message is for this system
if ((uint8_t) cmd.target_system == AC_ID) {
uint8_t result = MAV_RESULT_UNSUPPORTED;
switch (cmd.command) {
case MAV_CMD_NAV_GUIDED_ENABLE:
MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
result = MAV_RESULT_FAILED;
if (cmd.param1 > 0.5) {
autopilot_set_mode(AP_MODE_GUIDED);
if (autopilot_mode == AP_MODE_GUIDED) {
result = MAV_RESULT_ACCEPTED;
}
}
else {
// turn guided mode off - to what? maybe NAV? or MODE_AUTO2?
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
/* supposed to use this command to arm or SET_MODE?? */
MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
result = MAV_RESULT_FAILED;
if (cmd.param1 > 0.5) {
autopilot_set_motors_on(TRUE);
if (autopilot_motors_on)
result = MAV_RESULT_ACCEPTED;
}
else {
autopilot_set_motors_on(FALSE);
if (!autopilot_motors_on)
result = MAV_RESULT_ACCEPTED;
}
break;
default:
break;
}
// confirm command with result
mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result);
MAVLinkSendMessage();
}
break;
}
case MAVLINK_MSG_ID_SET_MODE: {
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(msg, &mode);
if (mode.target_system == AC_ID) {
MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode);
if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
autopilot_set_motors_on(TRUE);
}
else {
autopilot_set_motors_on(FALSE);
}
if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
autopilot_set_mode(AP_MODE_GUIDED);
}
else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
autopilot_set_mode(AP_MODE_NAV);
}
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
mavlink_set_position_target_local_ned_t target;
mavlink_msg_set_position_target_local_ned_decode(msg, &target);
// Check if this message is for this system
if (target.target_system == AC_ID) {
MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask);
/* if position and yaw bits are not set to ignored, use only position for now */
if (!(target.type_mask & 0b1110000000100000)) {
switch (target.coordinate_frame) {
case MAV_FRAME_LOCAL_NED:
MAVLINK_DEBUG("set position target, frame LOCAL_NED\n");
autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw);
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n");
autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw);
break;
case MAV_FRAME_BODY_OFFSET_NED:
MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n");
autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw);
break;
default:
break;
}
}
else if (!(target.type_mask & 0b0001110000100000)) {
/* position is set to ignore, but velocity not */
switch (target.coordinate_frame) {
case MAV_FRAME_LOCAL_NED:
MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n");
autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw);
break;
default:
break;
}
}
}
break;
}
#endif
default:
//Do nothing
MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
break;
}
}
void 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 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;
}
}
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 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 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
}