diff options
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink.c | 15 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_hil.h | 9 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp (renamed from src/modules/mavlink/mavlink_receiver.c) | 211 | ||||
-rw-r--r-- | src/modules/mavlink/missionlib.c | 175 | ||||
-rw-r--r-- | src/modules/mavlink/module.mk | 2 | ||||
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 41 | ||||
-rw-r--r-- | src/modules/mavlink/orb_topics.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink/waypoints.c | 66 |
8 files changed, 403 insertions, 120 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index de78cd139..5b8345e7e 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -144,14 +144,6 @@ set_hil_on_off(bool hil_enabled) /* Enable HIL */ if (hil_enabled && !mavlink_hil_enabled) { - /* Advertise topics */ - pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - - /* sensore level hil */ - pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ @@ -714,6 +706,8 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + /* sleep quarter the time */ usleep(25000); @@ -725,10 +719,13 @@ int mavlink_thread_main(int argc, char *argv[]) /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); /* sleep quarter the time */ usleep(25000); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + if (baudrate > 57600) { mavlink_pm_queued_send(); } @@ -787,7 +784,7 @@ int mavlink_main(int argc, char *argv[]) errx(0, "mavlink already running\n"); thread_should_exit = false; - mavlink_task = task_spawn("mavlink", + mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h index 8c7a5b514..744ed7d94 100644 --- a/src/modules/mavlink/mavlink_hil.h +++ b/src/modules/mavlink/mavlink_hil.h @@ -41,15 +41,6 @@ extern bool mavlink_hil_enabled; -extern struct vehicle_global_position_s hil_global_pos; -extern struct vehicle_attitude_s hil_attitude; -extern struct sensor_combined_s hil_sensors; -extern struct vehicle_gps_position_s hil_gps; -extern orb_advert_t pub_hil_global_pos; -extern orb_advert_t pub_hil_attitude; -extern orb_advert_t pub_hil_sensors; -extern orb_advert_t pub_hil_gps; - /** * Enable / disable Hardware in the Loop simulation mode. * diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.cpp index 940d030b2..33ac14860 100644 --- a/src/modules/mavlink/mavlink_receiver.c +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -49,7 +49,6 @@ #include <fcntl.h> #include <mqueue.h> #include <string.h> -#include "mavlink_bridge_header.h" #include <v1.0/common/mavlink.h> #include <drivers/drv_hrt.h> #include <time.h> @@ -62,10 +61,17 @@ #include <stdlib.h> #include <poll.h> +#include <mathlib/mathlib.h> + #include <systemlib/param/param.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/airspeed.h> #include <mavlink/mavlink_log.h> +__BEGIN_DECLS + +#include "mavlink_bridge_header.h" #include "waypoints.h" #include "orb_topics.h" #include "missionlib.h" @@ -73,8 +79,12 @@ #include "mavlink_parameters.h" #include "util.h" +extern bool gcs_link; + +__END_DECLS + /* XXX should be in a header somewhere */ -pthread_t receive_start(int uart); +extern "C" pthread_t receive_start(int uart); static void handle_message(mavlink_message_t *msg); static void *receive_thread(void *arg); @@ -88,18 +98,18 @@ 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; -orb_advert_t pub_hil_global_pos = -1; -orb_advert_t pub_hil_attitude = -1; -orb_advert_t pub_hil_gps = -1; -orb_advert_t pub_hil_sensors = -1; +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; +static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; - -extern bool gcs_link; +static orb_advert_t telemetry_status_pub = -1; static void handle_message(mavlink_message_t *msg) @@ -141,10 +151,10 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } - - /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } } } @@ -281,7 +291,7 @@ handle_message(mavlink_message_t *msg) } offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; + offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode); offboard_control_sp.timestamp = hrt_absolute_time(); @@ -296,6 +306,33 @@ handle_message(mavlink_message_t *msg) } } + /* handle status updates of the radio */ + if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + + struct telemetry_status_s tstatus; + + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + /* publish telemetry status topic */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (telemetry_status_pub == 0) { + telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus); + } + } + /* * Only decode hil messages in HIL mode. * @@ -308,10 +345,10 @@ handle_message(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); - if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { + if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - mavlink_highres_imu_t imu; - mavlink_msg_highres_imu_decode(msg, &imu); + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); /* packet counter */ static uint16_t hil_counter = 0; @@ -370,8 +407,34 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_counter = hil_counter; hil_sensors.accelerometer_counter = hil_counter; + /* differential pressure */ + hil_sensors.differential_pressure_pa = imu.diff_pressure; + hil_sensors.differential_pressure_counter = hil_counter; + + /* airspeed from differential pressure, ambient pressure and temp */ + struct airspeed_s airspeed; + airspeed.timestamp = hrt_absolute_time(); + + float ias = calc_indicated_airspeed(imu.diff_pressure); + // XXX need to fix this + float tas = ias; + + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (pub_hil_airspeed < 0) { + pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { + orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); + /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + if (pub_hil_sensors > 0) { + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { + pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + } // increment counters hil_counter++; @@ -379,21 +442,16 @@ handle_message(mavlink_message_t *msg) // output if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil imu at %d hz\n", hil_frames/10); + printf("receiving hil sensor at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } } - if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { + if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - mavlink_gps_raw_int_t gps; - mavlink_msg_gps_raw_int_decode(msg, &gps); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); /* gps */ hil_gps.timestamp_position = gps.time_usec; @@ -412,54 +470,40 @@ handle_message(mavlink_message_t *msg) /* go back to -PI..PI */ if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; - hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); - hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); - hil_gps.vel_d_m_s = 0.0f; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m + hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m + hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m hil_gps.vel_ned_valid = true; - /* COG (course over ground) is speced as -PI..+PI */ + /* COG (course over ground) is spec'ed as -PI..+PI */ hil_gps.cog_rad = heading_rad; hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - /* publish */ - orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; + /* publish GPS measurement data */ + if (pub_hil_gps > 0) { + orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { + pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + } - // output - // if ((timestamp - old_timestamp) > 10000000) { - // printf("receiving hil gps at %d hz\n", hil_frames/10); - // old_timestamp = timestamp; - // hil_frames = 0; - // } } - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { - mavlink_hil_state_t hil_state; - mavlink_msg_hil_state_decode(msg, &hil_state); + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - /* Calculate Rotation Matrix */ - //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode - - if (mavlink_system.type == MAV_TYPE_FIXED_WING) { - //TODO: assuming low pitch and roll values for now - hil_attitude.R[0][0] = cosf(hil_state.yaw); - hil_attitude.R[0][1] = sinf(hil_state.yaw); - hil_attitude.R[0][2] = 0.0f; - - hil_attitude.R[1][0] = -sinf(hil_state.yaw); - hil_attitude.R[1][1] = cosf(hil_state.yaw); - hil_attitude.R[1][2] = 0.0f; - - hil_attitude.R[2][0] = 0.0f; - hil_attitude.R[2][1] = 0.0f; - hil_attitude.R[2][2] = 1.0f; + struct airspeed_s airspeed; + airspeed.timestamp = hrt_absolute_time(); + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - hil_attitude.R_valid = true; + if (pub_hil_airspeed < 0) { + pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { + orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; @@ -468,21 +512,48 @@ handle_message(mavlink_message_t *msg) hil_global_pos.vy = hil_state.vy / 100.0f; hil_global_pos.vz = hil_state.vz / 100.0f; - /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); - hil_attitude.roll = hil_state.roll; - hil_attitude.pitch = hil_state.pitch; - hil_attitude.yaw = hil_state.yaw; + if (pub_hil_global_pos > 0) { + orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { + pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } + + /* Calculate Rotation Matrix */ + math::Quaternion q(hil_state.attitude_quaternion); + math::Dcm C_nb(q); + math::EulerAngles euler(C_nb); + + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + hil_attitude.R[i][j] = C_nb(i, j); + + hil_attitude.R_valid = true; + + /* set quaternion */ + hil_attitude.q[0] = q(0); + hil_attitude.q[1] = q(1); + hil_attitude.q[2] = q(2); + hil_attitude.q[3] = q(3); + hil_attitude.q_valid = true; + + hil_attitude.roll = euler.getPhi(); + hil_attitude.pitch = euler.getTheta(); + hil_attitude.yaw = euler.getPsi(); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; /* set timestamp and notify processes (broadcast) */ hil_attitude.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + + if (pub_hil_attitude > 0) { + orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } else { + pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { @@ -553,7 +624,9 @@ receive_thread(void *arg) while (!thread_should_exit) { - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; if (poll(fds, 1, timeout) > 0) { /* non-blocking read. read may return negative values */ @@ -592,7 +665,7 @@ receive_start(int uart) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index d369e05ff..4b010dd59 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -60,6 +60,7 @@ #include <stdlib.h> #include <poll.h> +#include <systemlib/err.h> #include <systemlib/param/param.h> #include <systemlib/systemlib.h> #include <mavlink/mavlink_log.h> @@ -73,6 +74,10 @@ #include "mavlink_parameters.h" static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; +static uint64_t loiter_start_time; + +static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, + struct vehicle_global_position_setpoint_s *sp); int mavlink_missionlib_send_message(mavlink_message_t *msg) @@ -123,6 +128,52 @@ uint64_t mavlink_missionlib_get_system_timestamp() } /** + * Set special vehicle setpoint fields based on current mission item. + * + * @return true if the mission item could be interpreted + * successfully, it return false on failure. + */ +bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, + struct vehicle_global_position_setpoint_s *sp) +{ + switch (command) { + case MAV_CMD_NAV_LOITER_UNLIM: + sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + break; + case MAV_CMD_NAV_LOITER_TIME: + sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + loiter_start_time = hrt_absolute_time(); + break; + // case MAV_CMD_NAV_LOITER_TURNS: + // sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT; + // break; + case MAV_CMD_NAV_WAYPOINT: + sp->nav_cmd = NAV_CMD_WAYPOINT; + break; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + break; + case MAV_CMD_NAV_LAND: + sp->nav_cmd = NAV_CMD_LAND; + break; + case MAV_CMD_NAV_TAKEOFF: + sp->nav_cmd = NAV_CMD_TAKEOFF; + break; + default: + /* abort */ + return false; + } + + sp->loiter_radius = param3; + sp->loiter_direction = (param3 >= 0) ? 1 : -1; + + sp->param1 = param1; + sp->param1 = param2; + sp->param1 = param3; + sp->param1 = param4; +} + +/** * This callback is executed each time a waypoint changes. * * It publishes the vehicle_global_position_setpoint_s or the @@ -133,9 +184,13 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { static orb_advert_t global_position_setpoint_pub = -1; + static orb_advert_t global_position_set_triplet_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; + static unsigned last_waypoint_index = -1; char buf[50] = {0}; + // XXX include check if WP is supported, jump to next if not + /* Update controller setpoints */ if (frame == (int)MAV_FRAME_GLOBAL) { /* global, absolute waypoint */ @@ -145,8 +200,9 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(param1, param2, param3, param4, command, &sp); - /* Initialize publication if necessary */ + /* Initialize setpoint publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); @@ -154,6 +210,113 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + + /* fill triplet: previous, current, next waypoint */ + struct vehicle_global_position_set_triplet_s triplet; + + /* current waypoint is same as sp */ + memcpy(&(triplet.current), &sp, sizeof(sp)); + + /* + * Check if previous WP (in mission, not in execution order) + * is available and identify correct index + */ + int last_setpoint_index = -1; + bool last_setpoint_valid = false; + + /* at first waypoint, but cycled once through mission */ + if (index == 0 && last_waypoint_index > 0) { + last_setpoint_index = last_waypoint_index; + } else { + last_setpoint_index = index - 1; + } + + while (last_setpoint_index >= 0) { + + if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && + (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + last_setpoint_valid = true; + break; + } + + last_setpoint_index--; + } + + /* + * Check if next WP (in mission, not in execution order) + * is available and identify correct index + */ + int next_setpoint_index = -1; + bool next_setpoint_valid = false; + + /* at last waypoint, try to re-loop through mission as default */ + if (index == (wpm->size - 1) && wpm->size > 1) { + next_setpoint_index = 0; + } else if (wpm->size > 1) { + next_setpoint_index = index + 1; + } + + while (next_setpoint_index < wpm->size - 1) { + + if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + next_setpoint_valid = true; + break; + } + + next_setpoint_index++; + } + + /* populate last and next */ + + triplet.previous_valid = false; + triplet.next_valid = false; + + if (last_setpoint_valid) { + triplet.previous_valid = true; + struct vehicle_global_position_setpoint_s sp; + sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; + sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; + sp.altitude = wpm->waypoints[last_setpoint_index].z; + sp.altitude_is_relative = false; + sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(wpm->waypoints[last_setpoint_index].param1, + wpm->waypoints[last_setpoint_index].param2, + wpm->waypoints[last_setpoint_index].param3, + wpm->waypoints[last_setpoint_index].param4, + wpm->waypoints[last_setpoint_index].command, &sp); + memcpy(&(triplet.previous), &sp, sizeof(sp)); + } + + if (next_setpoint_valid) { + triplet.next_valid = true; + struct vehicle_global_position_setpoint_s sp; + sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; + sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; + sp.altitude = wpm->waypoints[next_setpoint_index].z; + sp.altitude_is_relative = false; + sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(wpm->waypoints[next_setpoint_index].param1, + wpm->waypoints[next_setpoint_index].param2, + wpm->waypoints[next_setpoint_index].param3, + wpm->waypoints[next_setpoint_index].param4, + wpm->waypoints[next_setpoint_index].command, &sp); + memcpy(&(triplet.next), &sp, sizeof(sp)); + } + + /* Initialize triplet publication if necessary */ + if (global_position_set_triplet_pub < 0) { + global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); + + } else { + orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); + } + sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -164,6 +327,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { @@ -173,6 +337,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + + sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { @@ -192,8 +358,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, } sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + } else { + warnx("non-navigation WP, ignoring"); + mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); + return; } + /* only set this for known waypoint types (non-navigation types would have returned earlier) */ + last_waypoint_index = index; + mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index cbf08aeb2..bfccb2d38 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -40,7 +40,7 @@ SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ mavlink_log.c \ - mavlink_receiver.c \ + mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 295cd5e28..0597555ab 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -72,6 +72,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; +struct actuator_controls_effective_s actuators_0; +struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -116,6 +118,7 @@ static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); +static void l_airspeed(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -140,6 +143,7 @@ static const struct listener listeners[] = { {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, + {l_airspeed, &mavlink_subs.airspeed_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -192,7 +196,7 @@ l_sensor_combined(const struct listener *l) raw.gyro_rad_s[1], raw.gyro_rad_s[2], raw.magnetometer_ga[0], raw.magnetometer_ga[1], raw.magnetometer_ga[2], - raw.baro_pres_mbar, 0 /* no diff pressure yet */, + raw.baro_pres_mbar, raw.differential_pressure_pa, raw.baro_alt_meter, raw.baro_temp_celcius, fields_updated); @@ -202,9 +206,6 @@ l_sensor_combined(const struct listener *l) void l_vehicle_attitude(const struct listener *l) { - struct vehicle_attitude_s att; - - /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); @@ -564,28 +565,26 @@ l_manual_control_setpoint(const struct listener *l) void l_vehicle_attitude_controls(const struct listener *l) { - struct actuator_controls_effective_s actuators; - - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators.control_effective[0]); + actuators_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators.control_effective[1]); + actuators_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators.control_effective[2]); + actuators_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators.control_effective[3]); + actuators_0.control_effective[3]); } } @@ -626,6 +625,22 @@ l_home(const struct listener *l) mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); } +void +l_airspeed(const struct listener *l) +{ + struct airspeed_s airspeed; + + orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); + + float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.alt; + float climb = global_pos.vz; + + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, + ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); +} + static void * uorb_receive_thread(void *arg) { @@ -765,6 +780,10 @@ uorb_receive_start(void) mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ + /* --- AIRSPEED / VFR / HUD --- */ + mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index d61cd43dc..73e278dc6 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/optical_flow.h> @@ -59,7 +60,9 @@ #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/telemetry_status.h> #include <uORB/topics/debug_key_value.h> +#include <uORB/topics/airspeed.h> #include <drivers/drv_rc_input.h> struct mavlink_subscriptions { @@ -83,6 +86,7 @@ struct mavlink_subscriptions { int optical_flow; int rates_setpoint_sub; int home_sub; + int airspeed_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index a131b143b..cefcca468 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -347,15 +347,24 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ { static uint16_t counter; - // Do not flood the precious wireless link with debug data - // if (wpm->size > 0 && counter % 10 == 0) { - // printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); - // } + if (wpm->current_active_wp_id < wpm->size) { + float orbit; + if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { - if (wpm->current_active_wp_id < wpm->size) { + orbit = wpm->waypoints[wpm->current_active_wp_id].param2; + + } else if (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) { + + orbit = wpm->waypoints[wpm->current_active_wp_id].param3; + } else { + + // XXX set default orbit via param + orbit = 15.0f; + } - float orbit = wpm->waypoints[wpm->current_active_wp_id].param2; int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; @@ -374,10 +383,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw + wpm->pos_reached = true; - if (counter % 100 == 0) - printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); } // else @@ -394,29 +402,47 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (wpm->timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached - printf("Reached waypoint %u for the first time \n", cur_wp->seq); mavlink_wpm_send_waypoint_reached(cur_wp->seq); wpm->timestamp_firstinside_orbit = now; } // check if the MAV was long enough inside the waypoint orbit //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) { - printf("Reached waypoint %u long enough \n", cur_wp->seq); + bool time_elapsed = false; + + if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + time_elapsed = true; + } + } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + time_elapsed = true; + } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { + time_elapsed = true; + } + + if (time_elapsed) { if (cur_wp->autocontinue) { cur_wp->current = 0; - 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 - */ - wpm->current_active_wp_id = 0; + /* only accept supported navigation waypoints, skip unknown ones */ + do { - } else { - if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) - wpm->current_active_wp_id++; - } + 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 + */ + wpm->current_active_wp_id = 0; + + } else { + if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) + wpm->current_active_wp_id++; + } + + } while (!(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)); // Fly to next waypoint wpm->timestamp_firstinside_orbit = 0; |