aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink.c18
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp50
-rw-r--r--src/modules/mavlink/missionlib.c6
-rw-r--r--src/modules/mavlink/orb_listener.c25
-rw-r--r--src/modules/mavlink/orb_topics.h1
-rw-r--r--src/modules/mavlink/waypoints.c240
6 files changed, 129 insertions, 211 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 5eb7cba9b..7a5c2dab2 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -531,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[])
case 'b':
baudrate = strtoul(optarg, NULL, 10);
- if (baudrate == 0)
+ if (baudrate < 9600 || baudrate > 921600)
errx(1, "invalid baud rate '%s'", optarg);
break;
@@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[])
thread_running = false;
- exit(0);
+ return 0;
}
static void
@@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[])
/* this is not an error */
if (thread_running)
- errx(0, "mavlink already running\n");
+ errx(0, "mavlink already running");
thread_should_exit = false;
mavlink_task = task_spawn_cmd("mavlink",
@@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[])
2048,
mavlink_thread_main,
(const char **)argv);
+
+ while (!thread_running) {
+ usleep(200);
+ }
+
exit(0);
}
if (!strcmp(argv[1], "stop")) {
+
+ /* this is not an error */
+ if (!thread_running)
+ errx(0, "mavlink already stopped");
+
thread_should_exit = true;
while (thread_running) {
usleep(200000);
- printf(".");
+ warnx(".");
}
warnx("terminated.");
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4674f7a24..7dd9e321f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -102,6 +102,7 @@ struct vehicle_global_position_s hil_global_pos;
struct vehicle_attitude_s hil_attitude;
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
+struct battery_status_s hil_battery_status;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
@@ -111,6 +112,12 @@ static orb_advert_t pub_hil_accel = -1;
static orb_advert_t pub_hil_mag = -1;
static orb_advert_t pub_hil_baro = -1;
static orb_advert_t pub_hil_airspeed = -1;
+static orb_advert_t pub_hil_battery = -1;
+
+/* packet counter */
+static int hil_counter = 0;
+static int hil_frames = 0;
+static uint64_t old_timestamp = 0;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
@@ -149,7 +156,8 @@ handle_message(mavlink_message_t *msg)
vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
- vcmd.command = cmd_mavlink.command;
+ // XXX do proper translation
+ vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
@@ -207,7 +215,7 @@ handle_message(mavlink_message_t *msg)
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
- vcmd.command = MAV_CMD_DO_SET_MODE;
+ vcmd.command = VEHICLE_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
@@ -360,11 +368,6 @@ handle_message(mavlink_message_t *msg)
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
- /* packet counter */
- static uint16_t hil_counter = 0;
- static uint16_t hil_frames = 0;
- static uint64_t old_timestamp = 0;
-
/* sensors general */
hil_sensors.timestamp = hrt_absolute_time();
@@ -391,9 +394,9 @@ handle_message(mavlink_message_t *msg)
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
/* adc */
- hil_sensors.adc_voltage_v[0] = 0;
- hil_sensors.adc_voltage_v[1] = 0;
- hil_sensors.adc_voltage_v[2] = 0;
+ hil_sensors.adc_voltage_v[0] = 0.0f;
+ hil_sensors.adc_voltage_v[1] = 0.0f;
+ hil_sensors.adc_voltage_v[2] = 0.0f;
/* magnetometer */
float mga2ga = 1.0e-3f;
@@ -506,6 +509,18 @@ handle_message(mavlink_message_t *msg)
pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
}
+ /* fill in HIL battery status */
+ hil_battery_status.timestamp = hrt_absolute_time();
+ hil_battery_status.voltage_v = 11.1f;
+ hil_battery_status.current_a = 10.0f;
+
+ /* lazily publish the battery voltage */
+ if (pub_hil_battery > 0) {
+ orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+ } else {
+ pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+ }
+
// increment counters
hil_counter++;
hil_frames++;
@@ -574,6 +589,7 @@ handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
+ hil_global_pos.valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -639,6 +655,18 @@ handle_message(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
}
+
+ /* fill in HIL battery status */
+ hil_battery_status.timestamp = hrt_absolute_time();
+ hil_battery_status.voltage_v = 11.1f;
+ hil_battery_status.current_a = 10.0f;
+
+ /* lazily publish the battery voltage */
+ if (pub_hil_battery > 0) {
+ orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+ } else {
+ pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+ }
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
@@ -755,5 +783,7 @@ receive_start(int uart)
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+
+ pthread_attr_destroy(&receiveloop_attr);
return thread;
}
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index be88b8794..3175e64ce 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -167,9 +167,9 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
sp->loiter_direction = (param3 >= 0) ? 1 : -1;
sp->param1 = param1;
- sp->param1 = param2;
- sp->param1 = param3;
- sp->param1 = param4;
+ sp->param2 = param2;
+ sp->param3 = param3;
+ sp->param4 = param4;
}
/**
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 53d86ec00..cec2fdc43 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -99,6 +99,8 @@ struct listener {
uintptr_t arg;
};
+uint16_t cm_uint16_from_m_float(float m);
+
static void l_sensor_combined(const struct listener *l);
static void l_vehicle_attitude(const struct listener *l);
static void l_vehicle_gps_position(const struct listener *l);
@@ -150,6 +152,19 @@ static const struct listener listeners[] = {
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
+uint16_t
+cm_uint16_from_m_float(float m)
+{
+ if (m < 0.0f) {
+ return 0;
+
+ } else if (m > 655.35f) {
+ return 65535;
+ }
+
+ return (uint16_t)(m * 100.0f);
+}
+
void
l_sensor_combined(const struct listener *l)
{
@@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l)
/* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = gps.cog_rad;
+
if (cog_deg > M_PI_F)
cog_deg -= 2.0f * M_PI_F;
+
cog_deg *= M_RAD_TO_DEG_F;
@@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l)
gps.lat,
gps.lon,
gps.alt,
- gps.eph_m * 1e2f, // from m to cm
- gps.epv_m * 1e2f, // from m to cm
+ cm_uint16_from_m_float(gps.eph_m),
+ cm_uint16_from_m_float(gps.epv_m),
gps.vel_m_s * 1e2f, // from m/s to cm/s
- cog_deg * 1e2f, // from rad to deg * 100
+ cog_deg * 1e2f, // from deg to deg * 100
gps.satellites_visible);
/* update SAT info every 10 seconds */
@@ -829,5 +846,7 @@ uorb_receive_start(void)
pthread_t thread;
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
+
+ pthread_attr_destroy(&uorb_attr);
return thread;
}
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index e2e630046..c5cd0d03e 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -65,6 +65,7 @@
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
#include <drivers/drv_rc_input.h>
struct mavlink_subscriptions {
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 16a7c2d35..97472c233 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -246,47 +246,6 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
-//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
-//{
-// if (seq < wpm->size)
-// {
-// mavlink_mission_item_t *cur = waypoints->at(seq);
-//
-// const PxVector3 A(cur->x, cur->y, cur->z);
-// const PxVector3 C(x, y, z);
-//
-// // seq not the second last waypoint
-// if ((uint16_t)(seq+1) < wpm->size)
-// {
-// mavlink_mission_item_t *next = waypoints->at(seq+1);
-// const PxVector3 B(next->x, next->y, next->z);
-// const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
-// if (r >= 0 && r <= 1)
-// {
-// const PxVector3 P(A + r*(B-A));
-// return (P-C).length();
-// }
-// else if (r < 0.f)
-// {
-// return (C-A).length();
-// }
-// else
-// {
-// return (C-B).length();
-// }
-// }
-// else
-// {
-// return (C-A).length();
-// }
-// }
-// else
-// {
-// // if (verbose) // printf("ERROR: index out of bounds\n");
-// }
-// return -1.f;
-//}
-
/*
* Calculate distance in global frame.
*
@@ -337,7 +296,7 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
float dy = (cur->y - y);
float dz = (cur->z - z);
- return sqrt(dx * dx + dy * dy + dz * dz);
+ return sqrtf(dx * dx + dy * dy + dz * dz);
} else {
return -1.0f;
@@ -348,6 +307,11 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
{
static uint16_t counter;
+ if (!global_pos->valid && !local_pos->xy_valid) {
+ /* nothing to check here, return */
+ return;
+ }
+
if (wpm->current_active_wp_id < wpm->size) {
float orbit;
@@ -417,17 +381,57 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
}
if (time_elapsed) {
+
if (cur_wp->autocontinue) {
cur_wp->current = 0;
+ float navigation_lat = -1.0f;
+ float navigation_lon = -1.0f;
+ float navigation_alt = -1.0f;
+ int navigation_frame = -1;
+
+ /* initialize to current position in case we don't find a suitable navigation waypoint */
+ if (global_pos->valid) {
+ navigation_lat = global_pos->lat/1e7;
+ navigation_lon = global_pos->lon/1e7;
+ navigation_alt = global_pos->alt;
+ navigation_frame = MAV_FRAME_GLOBAL;
+ } else if (local_pos->xy_valid && local_pos->z_valid) {
+ navigation_lat = local_pos->x;
+ navigation_lon = local_pos->y;
+ navigation_alt = local_pos->z;
+ navigation_frame = MAV_FRAME_LOCAL_NED;
+ }
+
/* only accept supported navigation waypoints, skip unknown ones */
do {
+ /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
+ if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+ wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+ wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+ wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+
+ /* this is a navigation waypoint */
+ navigation_frame = cur_wp->frame;
+ navigation_lat = cur_wp->x;
+ navigation_lon = cur_wp->y;
+ navigation_alt = cur_wp->z;
+ }
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
- * activated restart the waypoint list from the beginning
+ * activated keep the system loitering there.
*/
- wpm->current_active_wp_id = 0;
+ cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+ cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+ cur_wp->frame = navigation_frame;
+ cur_wp->x = navigation_lat;
+ cur_wp->y = navigation_lon;
+ cur_wp->z = navigation_alt;
+ cur_wp->autocontinue = false;
+
+ /* we risk an endless loop for missions without navigation waypoints, abort. */
+ break;
} else {
if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
@@ -482,11 +486,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
}
}
- // Do NOT continously send the current WP, since we're not loosing messages
- // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) {
- // mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
- // }
-
check_waypoints_reached(now, global_position, local_position);
return OK;
@@ -498,115 +497,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
uint64_t now = mavlink_missionlib_get_system_timestamp();
switch (msg->msgid) {
-// case MAVLINK_MSG_ID_ATTITUDE:
-// {
-// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-// {
-// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
-// {
-// mavlink_attitude_t att;
-// mavlink_msg_attitude_decode(msg, &att);
-// float yaw_tolerance = wpm->accept_range_yaw;
-// //compare current yaw
-// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI)
-// {
-// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
-// wpm->yaw_reached = true;
-// }
-// else if(att.yaw - yaw_tolerance < 0.0f)
-// {
-// float lowerBound = 360.0f + att.yaw - yaw_tolerance;
-// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
-// wpm->yaw_reached = true;
-// }
-// else
-// {
-// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI;
-// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
-// wpm->yaw_reached = true;
-// }
-// }
-// }
-// break;
-// }
-//
-// case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
-// {
-// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-// {
-// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//
-// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
-// {
-// mavlink_local_position_ned_t pos;
-// mavlink_msg_local_position_ned_decode(msg, &pos);
-// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
-//
-// wpm->pos_reached = false;
-//
-// // compare current position (given in message) with current waypoint
-// float orbit = wp->param1;
-//
-// float dist;
-//// if (wp->param2 == 0)
-//// {
-//// // FIXME segment distance
-//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
-//// }
-//// else
-//// {
-// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z);
-//// }
-//
-// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached)
-// {
-// wpm->pos_reached = true;
-// }
-// }
-// }
-// break;
-// }
-
-// case MAVLINK_MSG_ID_CMD: // special action from ground station
-// {
-// mavlink_cmd_t action;
-// mavlink_msg_cmd_decode(msg, &action);
-// if(action.target == mavlink_system.sysid)
-// {
-// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
-// switch (action.action)
-// {
-// // case MAV_ACTION_LAUNCH:
-// // // if (verbose) std::cerr << "Launch received" << std::endl;
-// // current_active_wp_id = 0;
-// // if (wpm->size>0)
-// // {
-// // setActive(waypoints[current_active_wp_id]);
-// // }
-// // else
-// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
-// // break;
-//
-// // case MAV_ACTION_CONTINUE:
-// // // if (verbose) std::c
-// // err << "Continue received" << std::endl;
-// // idle = false;
-// // setActive(waypoints[current_active_wp_id]);
-// // break;
-//
-// // case MAV_ACTION_HALT:
-// // // if (verbose) std::cerr << "Halt received" << std::endl;
-// // idle = true;
-// // break;
-//
-// // default:
-// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
-// // break;
-// }
-// }
-// break;
-// }
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_mission_ack_t wpa;
@@ -617,26 +507,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
if (wpm->current_wp_id == wpm->size - 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
- mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#else
- if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
+ mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#endif
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
wpm->current_wp_id = 0;
}
}
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
}
break;
@@ -664,13 +544,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
}
}
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("NEW WP SET");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id);
-#endif
wpm->yaw_reached = false;
wpm->pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
@@ -678,33 +553,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
wpm->timestamp_firstinside_orbit = 0;
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
-
-#endif
}
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
-#endif
}
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
}
break;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 23:18:22 +0000