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;
}
}
int swarm_potential_task(void)
{
struct EnuCoor_f speed_sp = {.x = 0., .y = 0., .z = 0.};
// compute desired velocity
int8_t nb = 0;
uint8_t i;
if (gps.fix == 0) {return 1;}
struct UtmCoor_i my_pos;
my_pos.zone = 0;
utm_of_lla_i(&my_pos, &gps.lla_pos); // TODO update height to hmsl
for (i = 0; i < ti_acs_idx; i++) {
if (ti_acs[i].ac_id == 0 || ti_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(ti_acs[i].ac_id);
//float delta_t = ABS((int32_t)(gps.tow - ac->itow) / 1000.);
// if AC not responding for too long, continue, else compute force
//if(delta_t > 5) { continue; }
float de = (ac->utm.east - my_pos.east) / 100.; // + sha * delta_t
float dn = (ac->utm.north - my_pos.north) / 100.; // cha * delta_t
float da = (ac->utm.alt - my_pos.alt) / 1000.; // ac->climb * delta_t // currently wrong reference in other aircraft
float dist2 = de * de + dn * dn;// + da * da;
if (dist2 == 0.) {continue;}
float dist = sqrtf(dist2);
// potential force equation: x^2 - d0^3/x
float force = dist2 - TARGET_DIST3 / dist;
potential_force.east = (de * force) / dist;
potential_force.north = (dn * force) / dist;
potential_force.alt = (da * force) / dist;
// set carrot
// include speed of other vehicles for swarm cohesion
speed_sp.x += force_hor_gain * potential_force.east;// + ac->gspeed * sinf(ac->course);
speed_sp.y += force_hor_gain * potential_force.north;// + ac->gspeed * cosf(ac->course);
speed_sp.z += force_climb_gain * potential_force.alt;// + ac->climb;
nb++;
//debug
//potential_force.east = de;
//potential_force.north = dn;
//potential_force.alt = da;
}
// add waypoint force to get vehicle to waypoint
if (use_waypoint) {
struct EnuCoor_f my_enu = *stateGetPositionEnu_f();
float de = waypoint_get_x(SP_WP) - my_enu.x;
float dn = waypoint_get_y(SP_WP) - my_enu.y;
float da = waypoint_get_alt(SP_WP) - my_enu.z;
float dist2 = de * de + dn * dn;// + da * da;
if (dist2 > 0.01) { // add deadzone of 10cm from goal
float dist = sqrtf(dist2);
float force = 2 * dist2;
// higher attractive potential to get to goal when close by
if (dist < 1.) {
force = 2 * dist;
}
potential_force.east = force * de / dist;
potential_force.north = force * dn / dist;
potential_force.alt = force * da / dist;
speed_sp.x += force_hor_gain * potential_force.east;
speed_sp.y += force_hor_gain * potential_force.north;
speed_sp.z += force_climb_gain * potential_force.alt;
potential_force.east = de;
potential_force.north = dn;
potential_force.alt = force;
potential_force.speed = speed_sp.x;
potential_force.climb = speed_sp.y;
}
}
//potential_force.speed = speed_sp.x;
//potential_force.climb = speed_sp.y;
// limit commands
#ifdef GUIDANCE_H_REF_MAX_SPEED
BoundAbs(speed_sp.x, GUIDANCE_H_REF_MAX_SPEED);
BoundAbs(speed_sp.y, GUIDANCE_H_REF_MAX_SPEED);
BoundAbs(speed_sp.z, GUIDANCE_H_REF_MAX_SPEED);
#endif
potential_force.east = speed_sp.x;
potential_force.north = speed_sp.y;
potential_force.alt = speed_sp.z;
autopilot_guided_move_ned(speed_sp.y, speed_sp.x, 0, 0); // speed in enu
return 1;
}
