diff options
| -rw-r--r-- | apps/commander/commander.c | 390 | ||||
| -rw-r--r-- | apps/drivers/drv_mag.h | 5 | ||||
| -rw-r--r-- | apps/drivers/hmc5883/Makefile | 2 | ||||
| -rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 394 | ||||
| -rw-r--r-- | apps/drivers/l3gd20/l3gd20.cpp | 2 | ||||
| -rw-r--r-- | apps/mavlink/mavlink.c | 1491 | ||||
| -rw-r--r-- | apps/mavlink/mavlink_bridge_header.h | 18 | ||||
| -rw-r--r-- | apps/mavlink/mavlink_hil.h | 57 | ||||
| -rw-r--r-- | apps/mavlink/mavlink_parameters.c | 6 | ||||
| -rw-r--r-- | apps/mavlink/mavlink_parameters.h | 4 | ||||
| -rw-r--r-- | apps/mavlink/mavlink_receiver.c | 495 | ||||
| -rw-r--r-- | apps/mavlink/missionlib.c | 190 | ||||
| -rw-r--r-- | apps/mavlink/missionlib.h | 52 | ||||
| -rw-r--r-- | apps/mavlink/orb_listener.c | 640 | ||||
| -rw-r--r-- | apps/mavlink/orb_topics.h | 98 | ||||
| -rw-r--r-- | apps/mavlink/util.h | 54 | ||||
| -rw-r--r-- | apps/mavlink/waypoints.c | 14 | ||||
| -rw-r--r-- | apps/mavlink/waypoints.h | 1 | ||||
| -rw-r--r-- | apps/sdlog/sdlog.c | 3 | ||||
| -rw-r--r-- | apps/systemlib/getopt_long.h | 4 | ||||
| -rw-r--r-- | apps/uORB/uORB.h | 2 |
21 files changed, 2350 insertions, 1572 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 78fac5c22..43df6ceb2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -114,8 +114,8 @@ static bool commander_initialized = false; static struct vehicle_status_s current_status; /**< Main state machine */ static orb_advert_t stat_pub; -static uint16_t nofix_counter = 0; -static uint16_t gotfix_counter = 0; +// static uint16_t nofix_counter = 0; +// static uint16_t gotfix_counter = 0; static unsigned int failsafe_lowlevel_timeout_ms; @@ -124,7 +124,6 @@ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ /* pthread loops */ -static void *command_handling_loop(void *arg); static void *orb_receive_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -164,7 +163,7 @@ static void usage(const char *reason); * @param a The array to sort * @param n The number of entries in the array */ -static void cal_bsort(float a[], int n); +// static void cal_bsort(float a[], int n); static int buzzer_init() { @@ -293,7 +292,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) struct sensor_combined_s raw; /* 30 seconds */ - int calibration_interval_ms = 30 * 1000; + uint64_t calibration_interval = 30 * 1000 * 1000; unsigned int calibration_counter = 0; float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN}; @@ -308,42 +307,74 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 0.0f, 1.0f, }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag"); + } + close(fd); - mavlink_log_info(mavlink_fd, "[commander] Please rotate around X"); - uint64_t calibration_start = hrt_absolute_time(); - while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) { + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = 0; + + while (hrt_absolute_time() < calibration_deadline) { /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + /* user guidance */ + if (hrt_absolute_time() > axis_deadline && + axis_index < 3) { + char buf[50]; + sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + + axis_deadline += calibration_interval / 3; + axis_index++; + } + + if (!(axis_index < 3)) { + continue; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[commander] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + if (poll(fds, 1, 1000)) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - if (raw.magnetometer_ga[0] < mag_min[0]) { - mag_min[0] = raw.magnetometer_ga[0]; + /* ignore other axes */ + if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) { + mag_min[axis_index] = raw.magnetometer_ga[axis_index]; } - else if (raw.magnetometer_ga[0] > mag_max[0]) { - mag_max[0] = raw.magnetometer_ga[0]; + else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) { + mag_max[axis_index] = raw.magnetometer_ga[axis_index]; } - if (raw.magnetometer_ga[1] < mag_min[1]) { - mag_min[1] = raw.magnetometer_ga[1]; - } - else if (raw.magnetometer_ga[1] > mag_max[1]) { - mag_max[1] = raw.magnetometer_ga[1]; - } + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } - if (raw.magnetometer_ga[2] < mag_min[2]) { - mag_min[2] = raw.magnetometer_ga[2]; - } - else if (raw.magnetometer_ga[2] > mag_max[2]) { - mag_max[2] = raw.magnetometer_ga[2]; - } + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } calibration_counter++; } else { @@ -353,10 +384,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } } - usleep(200000); - - mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); - /* disable calibration mode */ status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); @@ -379,51 +406,49 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - if (!isfinite(mag_offset[0]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) - { - mavlink_log_info(mavlink_fd, "[commander] mag cal aborted: NaN"); - } - - //char buf[52]; - - //sprintf(buf, "mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); - //mavlink_log_info(mavlink_fd, buf); + if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) { - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } + /* announce and set new offset */ - if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); - } + if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); - } + if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } - fd = open(MAG_DEVICE_PATH, 0); - struct mag_scale mscale = { - mag_offset[0], - 1.0f, - mag_offset[1], - 1.0f, - mag_offset[2], - 1.0f, - }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - close(fd); + if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { + fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); + } - /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); - if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); + fd = open(MAG_DEVICE_PATH, 0); + struct mag_scale mscale = { + mag_offset[0], + 1.0f, + mag_offset[1], + 1.0f, + mag_offset[2], + 1.0f, + }; + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + close(fd); + + /* auto-save to EEPROM */ + int save_ret = pm_save_eeprom(false); + if(save_ret != 0) { + warn("WARNING: auto-save of params to EEPROM failed"); + } + + // char buf[50]; + // sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000)); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + } else { + mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); } - mavlink_log_info(mavlink_fd, "[commander] mag cal finished"); - close(sub_sensor_combined); } @@ -477,45 +502,51 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!"); - } - - if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!"); - } - - if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!"); - } + /* exit gyro calibration mode */ + status->flag_preflight_gyro_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - close(fd); + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); - if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); - } + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!"); + } + + if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!"); + } - /* exit to gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!"); + } - mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished"); + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + close(fd); + + /* auto-save to EEPROM */ + int save_ret = pm_save_eeprom(false); + if(save_ret != 0) { + warn("WARNING: auto-save of params to EEPROM failed"); + } - printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "[commander] gyro calibration done"); + } else { + mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)"); + } close(sub_sensor_combined); } @@ -570,61 +601,71 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) accel_offset[1] = accel_offset[1] / calibration_count; accel_offset[2] = accel_offset[2] / calibration_count; - /* add the removed length from x / y to z, since we induce a scaling issue else */ - float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]); + if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) { + + /* add the removed length from x / y to z, since we induce a scaling issue else */ + float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]); - /* if length is correct, zero results here */ - accel_offset[2] = accel_offset[2] + total_len; + /* if length is correct, zero results here */ + accel_offset[2] = accel_offset[2] + total_len; - float scale = 9.80665f / total_len; + float scale = 9.80665f / total_len; - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); - } - - if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); - } + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); + } + + if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); + } - if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); - } + if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + } - if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); - } - - if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); - } + if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); + } + + if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); + } - if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); - } + if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + } - fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offset[0], - scale, - accel_offset[1], - scale, - accel_offset[2], - scale, - }; - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - close(fd); + fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offset[0], + scale, + accel_offset[1], + scale, + accel_offset[2], + scale, + }; + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + close(fd); + + /* auto-save to EEPROM */ + int save_ret = pm_save_eeprom(false); + if(save_ret != 0) { + warn("WARNING: auto-save of params to EEPROM failed"); + } - /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); - if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); + //char buf[50]; + //sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "[commander] accel calibration done"); + } else { + mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)"); } - /* exit to gyro calibration mode */ + + /* exit accel calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished"); - printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]); + close(sub_sensor_combined); } @@ -742,8 +783,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -760,8 +803,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -823,37 +868,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -/** - * Handle commands sent by the ground control station via MAVLink. - */ -static void *command_handling_loop(void *arg) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander cmd handler", getpid()); - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); - } - } - - close(cmd_sub); - - return NULL; -} - static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal { /* Set thread name */ @@ -1002,7 +1016,7 @@ int commander_thread_main(int argc, char *argv[]) printf("[commander] I am in command now!\n"); /* pthreads for command and subsystem info handling */ - pthread_t command_handling_thread; + // pthread_t command_handling_thread; pthread_t subsystem_info_thread; /* initialize */ @@ -1044,11 +1058,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[commander] system is running"); /* create pthreads */ - pthread_attr_t command_handling_attr; - pthread_attr_init(&command_handling_attr); - pthread_attr_setstacksize(&command_handling_attr, 6000); - pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL); - pthread_attr_t subsystem_info_attr; pthread_attr_init(&subsystem_info_attr); pthread_attr_setstacksize(&subsystem_info_attr, 2048); @@ -1093,6 +1102,11 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1122,6 +1136,15 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + orb_check(cmd_sub, &new_data); + if (new_data) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(stat_pub, ¤t_status, &cmd); + } + battery_voltage = sensors.battery_voltage_v; battery_voltage_valid = sensors.battery_voltage_valid; @@ -1480,7 +1503,7 @@ int commander_thread_main(int argc, char *argv[]) } /* wait for threads to complete */ - pthread_join(command_handling_thread, NULL); + // pthread_join(command_handling_thread, NULL); pthread_join(subsystem_info_thread, NULL); /* close fds */ @@ -1490,6 +1513,7 @@ int commander_thread_main(int argc, char *argv[]) close(sp_offboard_sub); close(gps_sub); close(sensor_sub); + close(cmd_sub); printf("[commander] exiting..\n"); fflush(stdout); diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 1f5bb998e..88381aaaa 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -99,7 +99,10 @@ ORB_DECLARE(sensor_mag); /** copy the mag scaling constants to the structure pointed to by (arg) */ #define MAGIOCGSCALE _MAGIOC(3) +/** set the measurement range to handle (at least) arg Gauss */ +#define MAGIOCSRANGE _MAGIOC(4) + /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(4) +#define MAGIOCCALIBRATE _MAGIOC(5) #endif /* _DRV_MAG_H */ diff --git a/apps/drivers/hmc5883/Makefile b/apps/drivers/hmc5883/Makefile index aa4a397fb..4d7cb4e7b 100644 --- a/apps/drivers/hmc5883/Makefile +++ b/apps/drivers/hmc5883/Makefile @@ -37,6 +37,6 @@ APPNAME = hmc5883 PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index c0a5f4049..893119407 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -176,6 +176,24 @@ private: void stop(); /** + * Perform the on-sensor scale calibration routine. + * + * @note The sensor will continue to provide measurements, these + * will however reflect the uncalibrated sensor state until + * the calibration routine has been completed. + * + * @param enable set to 1 to enable self-test strap, 0 to disable + */ + int calibrate(unsigned enable); + + /** + * Set the sensor range. + * + * Sets the internal range to handle at least the argument in Gauss. + */ + int set_range(unsigned range); + + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. * @@ -255,8 +273,8 @@ HMC5883::HMC5883(int bus) : _oldest_report(0), _reports(nullptr), _mag_topic(-1), - _range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */ - _range_ga(0.88f), + _range_scale(0), /* default range scale from counts to gauss */ + _range_ga(1.3f), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")) @@ -308,11 +326,71 @@ HMC5883::init() if (_mag_topic < 0) debug("failed to create sensor_mag object"); + /* set range */ + set_range(_range_ga); + ret = OK; out: return ret; } +int HMC5883::set_range(unsigned range) +{ + uint8_t range_bits; + + if (range < 1) { + range_bits = 0x00; + _range_scale = 1.0f / 1370.0f; + _range_ga = 0.88f; + } else if (range <= 1) { + range_bits = 0x01; + _range_scale = 1.0f / 1090.0f; + _range_ga = 1.3f; + } else if (range <= 2) { + range_bits = 0x02; + _range_scale = 1.0f / 820.0f; + _range_ga = 1.9f; + } else if (range <= 3) { + range_bits = 0x03; + _range_scale = 1.0f / 660.0f; + _range_ga = 2.5f; + } else if (range <= 4) { + range_bits = 0x04; + _range_scale = 1.0f / 440.0f; + _range_ga = 4.0f; + } else if (range <= 4.7f) { + range_bits = 0x05; + _range_scale = 1.0f / 390.0f; + _range_ga = 4.7f; + } else if (range <= 5.6f) { + range_bits = 0x06; + _range_scale = 1.0f / 330.0f; + _range_ga = 5.6f; + } else { + range_bits = 0x07; + _range_scale = 1.0f / 230.0f; + _range_ga = 8.1f; + } + + int ret; + + /* + * Send the command to set the range + */ + ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + + if (OK != ret) + perf_count(_comms_errors); + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + + if (OK != ret) + perf_count(_comms_errors); + + return !(range_bits_in == (range_bits << 5)); +} + int HMC5883::probe() { @@ -495,6 +573,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) /* not supported, always 1 sample per poll */ return -EINVAL; + case MAGIOCSRANGE: + return set_range(arg); + case MAGIOCSLOWPASS: /* not supported, no internal filtering */ return -EINVAL; @@ -510,8 +591,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 0; case MAGIOCCALIBRATE: - /* XXX perform auto-calibration */ - return -EINVAL; + return calibrate(arg); default: /* give it to the superclass */ @@ -718,6 +798,29 @@ out: return ret; } +int HMC5883::calibrate(unsigned enable) +{ + int ret; + /* arm the excitement strap */ + uint8_t conf_reg; + ret = read_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) + perf_count(_comms_errors); + if (enable) { + conf_reg |= 0x01; + } else { + conf_reg &= ~0x03; + } + ret = write_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) + perf_count(_comms_errors); + + uint8_t conf_reg_ret; + read_reg(ADDR_CONF_A, conf_reg_ret); + + return !(conf_reg == conf_reg_ret); +} + int HMC5883::write_reg(uint8_t reg, uint8_t val) { @@ -775,6 +878,7 @@ void start(); void test(); void reset(); void info(); +int calibrate(); /** * Start the driver. @@ -872,6 +976,273 @@ test() errx(0, "PASS"); } + +/** + * Automatic scale calibration. + * + * Basic idea: + * + * output = (ext field +- 1.1 Ga self-test) * scale factor + * + * and consequently: + * + * 1.1 Ga = (excited - normal) * scale factor + * scale factor = (excited - normal) / 1.1 Ga + * + * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 + * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 + * + * By subtracting the non-excited measurement the pure 1.1 Ga reading + * can be extracted and the sensitivity of all axes can be matched. + * + * SELF TEST OPERATION + * To check the HMC5883L for proper operation, a self test feature in incorporated + * in which the sensor offset straps are excited to create a nominal field strength + * (bias field) to be measured. To implement self test, the least significant bits + * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias) + * or 10 (negetive bias), e.g. 0x11 or 0x12. + * Then, by placing the mode register into single-measurement mode (0x01), + * two data acquisition cycles will be made on each magnetic vector. + * The first acquisition will be a set pulse followed shortly by measurement + * data of the external field. The second acquisition will have the offset strap + * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create + * about a ±1.1 gauss self test field plus the external field. The first acquisition + * values will be subtracted from the second acquisition, and the net measurement + * will be placed into the data output registers. + * Since self test adds ~1.1 Gauss additional field to the existing field strength, + * using a reduced gain setting prevents sensor from being saturated and data registers + * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3), + * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data + * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data + * output register. To leave the self test mode, change MS1 and MS0 bit of the + * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. + * Using the self test method described above, the user can scale sensor + */ +int calibrate() +{ + + struct mag_report report; + ssize_t sz; + int ret; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); + + /* start the sensor polling at 10 Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) + errx(1, "failed to set 2Hz poll rate"); + + /* Set to 2.5 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { + warnx("failed to set 2.5 Ga range"); + } + + if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { + warnx("failed to enable sensor calibration mode"); + } + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set null scale / offsets for mag"); + } + + float avg_excited[3]; + unsigned i; + + /* read the sensor 10x and report each value */ + for (i = 0; i < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } else { + avg_excited[0] += report.x; + avg_excited[1] += report.y; + avg_excited[2] += report.z; + } + + warnx("periodic read %u", i); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + } + + // warnx("starting calibration"); + + // struct mag_report report; + // ssize_t sz; + // int ret; + + // int fd = open(MAG_DEVICE_PATH, O_RDONLY); + // if (fd < 0) + // err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + // /* do a simple demand read */ + // sz = read(fd, &report, sizeof(report)); + // if (sz != sizeof(report)) + // err(1, "immediate read failed"); + + // warnx("single read"); + // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + // warnx("time: %lld", report.timestamp); + + // /* get scaling, set to zero */ + // struct mag_scale mscale_previous; + + // if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { + // warn("WARNING: failed to get scale / offsets for mag"); + // } + + // struct mag_scale mscale_null = { + // 0.0f, + // 1.0f, + // 0.0f, + // 1.0f, + // 0.0f, + // 1.0f, + // }; + + // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + // warn("WARNING: failed to set null scale / offsets for mag"); + // } + + // warnx("sensor ready"); + + // float avg_excited[3] = {0.0f, 0.0f, 0.0f}; + + // if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { + // warnx("failed to enable sensor calibration mode"); + // } + + // /* Set to 2.5 Gauss */ + // if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { + // warnx("failed to set 2.5 Ga range"); + // } + + // /* set the queue depth to 10 */ + // if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + // warnx("failed to set queue depth"); + // return 1; + // } else { + // warnx("set queue depth"); + // } + + // /* start the sensor polling at 100Hz */ + // if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 100)) { + // warnx("failed to set 100 Hz poll rate"); + // return 1; + // } else { + // warnx("set 100 Hz poll rate"); + // } + + // int i; + // for (i = 0; i < 10; i++) { + // struct pollfd fds; + + // (void) ioctl(fd, MAGIOCCALIBRATE, 1); + + // /* wait for data to be ready */ + // fds.fd = fd; + // fds.events = POLLIN; + // ret = poll(&fds, 1, 2000); + + // if (ret != 1) { + // warnx("timed out waiting for sensor data"); + // return 1; + // } + + // /* now go get it */ + // sz = read(fd, &report, sizeof(report)); + + // if (sz != sizeof(report)) { + // warn("periodic read failed"); + // return 1; + // } else { + // avg_excited[0] += report.x; + // avg_excited[1] += report.y; + // avg_excited[2] += report.z; + // } + // warnx("excited read %u", i); + // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + // warnx("time: %lld", report.timestamp); + + // } + + avg_excited[0] /= i; + avg_excited[1] /= i; + avg_excited[2] /= i; + + warnx("periodic excited reads %u", i); + warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); + + /* Set to 1.1 Gauss and end calibration */ + ret = ioctl(fd, MAGIOCCALIBRATE, 0); + ret = ioctl(fd, MAGIOCSRANGE, 1); + + float scaling[3]; + + /* calculate axis scaling */ + scaling[0] = 1.16f / avg_excited[0]; + /* second axis inverted */ + scaling[1] = 1.16f / -avg_excited[1]; + scaling[2] = 1.08f / avg_excited[2]; + + warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); + + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + } + + if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) { + warnx("failed to disable sensor calibration mode"); + } + + /* set scaling in device */ + // mscale_previous.x_scale = scaling[0]; + // mscale_previous.y_scale = scaling[1]; + // mscale_previous.z_scale = scaling[2]; + + // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + // warn("WARNING: failed to set new scale / offsets for mag"); + // } + + errx(0, "PASS"); +} + /** * Reset the driver. */ @@ -930,8 +1301,19 @@ hmc5883_main(int argc, char *argv[]) /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) hmc5883::info(); - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); + /* + * Autocalibrate the scaling + */ + if (!strcmp(argv[1], "calibrate")) { + if (hmc5883::calibrate() == 0) { + errx(0, "calibration successful"); + } else { + errx(1, "calibration failed"); + } + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); } diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 9401fdd4a..e9b60b01c 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -864,5 +864,5 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(argv[1], "info")) l3gd20::info(); - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 0592d0377..e893ea951 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -58,30 +58,19 @@ #include <errno.h> #include <stdlib.h> #include <poll.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/vehicle_command.h> -#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_attitude_setpoint.h> -#include <uORB/topics/optical_flow.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/debug_key_value.h> #include <systemlib/param/param.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> #include "waypoints.h" #include "mavlink_log.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" +#include "waypoints.h" +#include "mavlink_parameters.h" /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); @@ -89,64 +78,48 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_QUADROTOR); __EXPORT int mavlink_main(int argc, char *argv[]); -int mavlink_thread_main(int argc, char *argv[]); - -static bool thread_should_exit = false; -static bool thread_running = false; -static int mavlink_task; -/* terminate MAVLink on user request - disabled by default */ -static bool mavlink_link_termination_allowed = false; +static int mavlink_thread_main(int argc, char *argv[]); -mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_QUADROTOR, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255 -static uint8_t chan = MAVLINK_COMM_0; -static mavlink_status_t status; +/* thread state */ +volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int mavlink_task; /* pthreads */ static pthread_t receive_thread; static pthread_t uorb_receive_thread; -/* Allocate storage space for waypoints */ -mavlink_wpm_storage wpm_s; - -/** Global position */ -static struct vehicle_global_position_s global_pos; - -/** Local position */ -static struct vehicle_local_position_s local_pos; - -/** Vehicle status */ -static struct vehicle_status_s v_status; - -/** RC channels */ -static struct rc_channels_s rc; - -/* HIL publishers */ -static orb_advert_t pub_hil_attitude = -1; - -/** HIL attitude */ -static struct vehicle_attitude_s hil_attitude; - -static struct vehicle_global_position_s hil_global_pos; +/* terminate MAVLink on user request - disabled by default */ +static bool mavlink_link_termination_allowed = false; -static struct offboard_control_setpoint_s offboard_control_sp; +mavlink_system_t mavlink_system = { + 100, + 50, + MAV_TYPE_QUADROTOR, + 0, + 0, + 0 +}; // System ID, 1-255, Component/Subsystem ID, 1-255 -static struct vehicle_command_s vcmd; +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; -static struct actuator_armed_s armed; +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); -static struct vehicle_vicon_position_s vicon_position; +/* Allocate storage space for waypoints */ +static mavlink_wpm_storage wpm_s; +mavlink_wpm_storage *wpm = &wpm_s; -static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; -static orb_advert_t global_position_setpoint_pub = -1; -static orb_advert_t local_position_setpoint_pub = -1; -static bool mavlink_hil_enabled = false; +bool mavlink_hil_enabled = false; +/* buffer for message strings */ static char mavlink_message_string[51] = {0}; -static int baudrate = 57600; +/* protocol interface */ +static int uart; +static int baudrate; /* interface mode */ static enum { @@ -154,207 +127,14 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; -static struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int armed_sub; - int actuators_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int spg_sub; - int debug_key_value; - bool initialized; -} mavlink_subs = { - .sensor_sub = 0, - .att_sub = 0, - .global_pos_sub = 0, - .act_0_sub = 0, - .act_1_sub = 0, - .act_2_sub = 0, - .act_3_sub = 0, - .gps_sub = 0, - .man_control_sp_sub = 0, - .armed_sub = 0, - .actuators_sub = 0, - .local_pos_sub = 0, - .spa_sub = 0, - .spl_sub = 0, - .spg_sub = 0, - .debug_key_value = 0, - .initialized = false -}; - -static struct mavlink_publications { - orb_advert_t offboard_control_sp_pub; - orb_advert_t vicon_position_pub; -} mavlink_pubs = { - .offboard_control_sp_pub = -1, - .vicon_position_pub = -1 -}; - - -/* 3: Define waypoint helper functions */ -void mavlink_wpm_send_message(mavlink_message_t *msg); -void mavlink_wpm_send_gcs_string(const char *string); -uint64_t mavlink_wpm_get_system_timestamp(void); -int mavlink_missionlib_send_message(mavlink_message_t *msg); -int mavlink_missionlib_send_gcs_string(const char *string); -uint64_t mavlink_missionlib_get_system_timestamp(void); - -void handleMessage(mavlink_message_t *msg); static void mavlink_update_system(void); +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +static void usage(void); -/** - * Enable / disable Hardware in the Loop simulation mode. - */ -int set_hil_on_off(bool hil_enabled); - -/** - * Translate the custom state into standard mavlink modes and state. - */ -void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const struct actuator_armed_s *actuator, uint8_t *mavlink_state, uint8_t *mavlink_mode); - -int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - -/* 4: Include waypoint protocol */ -#include "waypoints.h" -mavlink_wpm_storage *wpm; - - -#include "mavlink_parameters.h" - -/** - * Print the usage - */ -static void usage(const char *reason); - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; -int mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - int writelen = write(uart, missionlib_msg_buf, len); - if (writelen != len) { - return 1; - } else { - return 0; - } -} -int mavlink_missionlib_send_gcs_string(const char *string) -{ - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') - break; - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - return mavlink_missionlib_send_message(&msg); - } else { - return 1; - } -} - -/** - * Get system time since boot in microseconds - * - * @return the system time since boot in microseconds - */ -uint64_t mavlink_missionlib_get_system_timestamp() -{ - return hrt_absolute_time(); -} - -/** - * This callback is executed each time a waypoint changes. - * - * It publishes the vehicle_global_position_setpoint_s or the - * vehicle_local_position_setpoint_s topic, depending on the type of waypoint - */ -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) -{ - char buf[50] = {0}; - - /* Update controller setpoints */ - if (frame == (int)MAV_FRAME_GLOBAL) { - /* global, absolute waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = false; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - 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) { - /* global, relative alt (in relation to HOME) waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = true; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - } else { - 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) { - /* local, absolute waypoint */ - struct vehicle_local_position_setpoint_s sp; - sp.x = param5_lat_x; - sp.y = param6_lon_y; - sp.z = param7_alt_z; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - /* Initialize publication if necessary */ - if (local_position_setpoint_pub < 0) { - local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); - } - 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); - } - - 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); -} - - - -int set_hil_on_off(bool hil_enabled) +int +set_hil_on_off(bool hil_enabled) { int ret = OK; @@ -363,9 +143,6 @@ int set_hil_on_off(bool hil_enabled) //printf("\n HIL ON \n"); - (void)close(pub_hil_attitude); - (void)close(pub_hil_global_pos); - /* 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); @@ -401,8 +178,6 @@ int set_hil_on_off(bool hil_enabled) if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; orb_set_interval(mavlink_subs.spa_sub, 200); - (void)close(pub_hil_attitude); - (void)close(pub_hil_global_pos); } else { ret = ERROR; @@ -411,32 +186,33 @@ int set_hil_on_off(bool hil_enabled) return ret; } -void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const struct actuator_armed_s *actuator, uint8_t *mavlink_state, uint8_t *mavlink_mode) +void +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (c_status->flag_control_manual_enabled) { + if (v_status.flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (c_status->flag_hil_enabled) { + if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (actuator->armed) { + if (armed.armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (c_status->state_machine) { + switch (v_status.state_machine) { case SYSTEM_STATE_PREFLIGHT: - if (c_status->flag_preflight_gyro_calibration || - c_status->flag_preflight_mag_calibration || - c_status->flag_preflight_accel_calibration) { + if (v_status.flag_preflight_gyro_calibration || + v_status.flag_preflight_mag_calibration || + v_status.flag_preflight_accel_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; } else { *mavlink_state = MAV_STATE_UNINIT; @@ -489,47 +265,6 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const s } -/** - * Receive data from UART. - */ -static void *receiveloop(void *arg) -{ - int uart_fd = *((int*)arg); - - const int timeout = 1000; - uint8_t ch; - - mavlink_message_t msg; - - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); - - while (!thread_should_exit) { - - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - - if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; - - do { - nread = read(uart_fd, &ch, 1); - - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char - /* handle generic messages and commands */ - handleMessage(&msg); - - /* Handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - - /* Handle packet with parameter component */ - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); - } - } while (nread > 0); - } - } - - return NULL; -} static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { @@ -537,35 +272,35 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma switch (mavlink_msg_id) { case MAVLINK_MSG_ID_SCALED_IMU: - /* senser sub triggers scaled IMU */ - if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); + /* sensor sub triggers scaled IMU */ + orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_HIGHRES_IMU: - /* senser sub triggers highres IMU */ - if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); + /* sensor sub triggers highres IMU */ + orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_RAW_IMU: - /* senser sub triggers RAW IMU */ - if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); + /* sensor sub triggers RAW IMU */ + orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_ATTITUDE: /* attitude sub triggers attitude */ - if (subs->att_sub) orb_set_interval(subs->att_sub, min_interval); + orb_set_interval(subs->att_sub, min_interval); break; case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: /* actuator_outputs triggers this message */ - if (subs->act_0_sub) orb_set_interval(subs->act_0_sub, min_interval); - if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval); - if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval); - if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval); - if (subs->actuators_sub) orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->act_0_sub, min_interval); + orb_set_interval(subs->act_1_sub, min_interval); + orb_set_interval(subs->act_2_sub, min_interval); + orb_set_interval(subs->act_3_sub, min_interval); + orb_set_interval(subs->actuators_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: /* manual_control_setpoint triggers this message */ - if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval); + orb_set_interval(subs->man_control_sp_sub, min_interval); break; case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - if (subs->debug_key_value) orb_set_interval(subs->debug_key_value, min_interval); + orb_set_interval(subs->debug_key_value, min_interval); break; default: /* not found */ @@ -576,546 +311,6 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma return ret; } -/** - * Listen for uORB topics and send via MAVLink. - * - * This pthread performs a blocking wait on selected - * uORB topics and sends them via MAVLink to other - * vehicles or a ground control station. - */ -static void *uorb_receiveloop(void *arg) -{ - /* obtain reference to task's subscriptions */ - struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg; - - /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); - - - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 25; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - - - union { - struct sensor_combined_s raw; - struct vehicle_attitude_s att; - struct vehicle_gps_position_s gps; - struct vehicle_local_position_setpoint_s local_sp; - struct vehicle_global_position_setpoint_s global_sp; - struct vehicle_attitude_setpoint_s att_sp; - struct actuator_outputs_s act_outputs; - struct manual_control_setpoint_s man_control; - struct actuator_controls_s actuators; - struct debug_key_value_s debug; - } buf; - - /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ - subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = subs->sensor_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ - subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(subs->att_sub, 100); - fds[fdsc_count].fd = subs->att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS VALUE --- */ - /* subscribe to ORB for attitude */ - subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(subs->gps_sub, 1000); /* 1Hz updates */ - fds[fdsc_count].fd = subs->gps_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- SYSTEM STATE --- */ - /* struct already globally allocated */ - /* subscribe to topic */ - int status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - fds[fdsc_count].fd = status_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- RC CHANNELS VALUE --- */ - /* struct already globally allocated */ - /* subscribe to ORB for global position */ - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(rc_sub, 100); /* 10Hz updates */ - fds[fdsc_count].fd = rc_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POS VALUE --- */ - /* struct already globally allocated and topic already subscribed */ - fds[fdsc_count].fd = subs->global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POS VALUE --- */ - /* struct and topic already globally subscribed */ - fds[fdsc_count].fd = subs->local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL SETPOINT VALUE --- */ - /* subscribe to ORB for local setpoint */ - /* struct already allocated */ - subs->spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - orb_set_interval(subs->spg_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spg_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL SETPOINT VALUE --- */ - /* subscribe to ORB for local setpoint */ - /* struct already allocated */ - subs->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(subs->spl_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spl_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ - subs->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(subs->spa_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spa_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- ACTUATOR OUTPUTS --- */ - subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs->act_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - fds[fdsc_count].fd = subs->act_1_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - fds[fdsc_count].fd = subs->act_2_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); - fds[fdsc_count].fd = subs->act_3_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- MAPPED MANUAL CONTROL INPUTS --- */ - subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - fds[fdsc_count].fd = subs->man_control_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR ARMED VALUE --- */ - /* subscribe to ORB for actuator armed */ - subs->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - fds[fdsc_count].fd = subs->armed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs->actuators_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- DEBUG VALUE OUTPUT --- */ - /* subscribe to ORB for debug value outputs */ - subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - fds[fdsc_count].fd = subs->debug_key_value; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* all subscriptions initialized, return success */ - subs->initialized = true; - - unsigned int sensors_raw_counter = 0; - unsigned int attitude_counter = 0; - unsigned int gps_counter = 0; - - /* WARNING: If you get the error message below, - * then the number of registered messages (fdsc) - * differs from the number of messages in the above list. - */ - if (fdsc_count > fdsc) { - fprintf(stderr, "[mavlink] WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; - - /* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ - uint64_t last_sensor_timestamp = 0; - - while (!thread_should_exit) { - - int poll_ret = poll(fds, fdsc_count, timeout); - - /* handle the poll result */ - if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); - } else if (poll_ret < 0) { - mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); - } else { - - int ifds = 0; - - /* --- SENSORS RAW VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw); - - last_sensor_timestamp = buf.raw.timestamp; - - /* send raw imu data */ - // mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0], - // buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], - // buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], - // buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); - - /* mark individual fields as changed */ - uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; - - if (accel_counter != buf.raw.accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = buf.raw.accelerometer_counter; - } - - if (gyro_counter != buf.raw.gyro_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = buf.raw.gyro_counter; - } - - if (mag_counter != buf.raw.magnetometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = buf.raw.magnetometer_counter; - } - - if (baro_counter != buf.raw.baro_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = buf.raw.baro_counter; - } - - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], - buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0], - buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2], - buf.raw.magnetometer_ga[0], - buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2], - buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */, - buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius, - fields_updated); - /* send pressure */ - //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100); - - sensors_raw_counter++; - } - - /* --- ATTITUDE VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att); - - /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); - - attitude_counter++; - } - - /* --- GPS VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps); - /* GPS position */ - mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible); - - if (buf.gps.satellite_info_available && (gps_counter % 4 == 0)) { - mavlink_msg_gps_status_send(MAVLINK_COMM_0, buf.gps.satellites_visible, buf.gps.satellite_prn, buf.gps.satellite_used, buf.gps.satellite_elevation, buf.gps.satellite_azimuth, buf.gps.satellite_snr); - } - - gps_counter++; - } - - /* --- SYSTEM STATUS --- */ - if (fds[ifds++].revents & POLLIN) { - /* immediately communicate state changes back to user */ - orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); - /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); - } - - /* --- RC CHANNELS --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw, - rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi); - } - - /* --- VEHICLE GLOBAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos); - uint64_t timestamp = global_pos.timestamp; - int32_t lat = global_pos.lat; - int32_t lon = global_pos.lon; - int32_t alt = (int32_t)(global_pos.alt*1000); - int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); - int16_t vx = (int16_t)(global_pos.vx * 100.0f); - int16_t vy = (int16_t)(global_pos.vy * 100.0f); - int16_t vz = (int16_t)(global_pos.vz * 100.0f); - /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); - - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, - relative_alt, vx, vy, vz, hdg); - } - - /* --- VEHICLE LOCAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), subs->local_pos_sub, &local_pos); - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, - local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz); - } - - /* --- VEHICLE GLOBAL SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position_setpoint), subs->spg_sub, &buf.global_sp); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - if (buf.global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat, - buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw); - } - - /* --- VEHICLE LOCAL SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), subs->spl_sub, &buf.local_sp); - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, - buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw); - } - - /* --- VEHICLE ATTITUDE SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp); - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, - buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); - } - - /* --- ACTUATOR OUTPUTS 0 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 0 /* port 0 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - - // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), - // 1 /* port 1 */, - // buf.act_outputs.output[ 8], - // buf.act_outputs.output[ 9], - // buf.act_outputs.output[10], - // buf.act_outputs.output[11], - // buf.act_outputs.output[12], - // buf.act_outputs.output[13], - // buf.act_outputs.output[14], - // buf.act_outputs.output[15]); - // } - } - - /* --- ACTUATOR OUTPUTS 1 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 3 /* port 3 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- ACTUATOR OUTPUTS 2 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 5 /* port 5 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- ACTUATOR OUTPUTS 3 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 7 /* port 7 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000, - buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0); - } - - /* --- ACTUATOR ARMED --- */ - if (fds[ifds++].revents & POLLIN) { - } - - /* --- ACTUATOR CONTROL --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators); - /* 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, "ctrl0 ", buf.actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]); - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - buf.actuators.control[0], - buf.actuators.control[1], - buf.actuators.control[2], - buf.actuators.control[3], - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } - } - - /* --- DEBUG KEY/VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug); - /* Enforce null termination */ - buf.debug.key[sizeof(buf.debug.key) - 1] = '\0'; - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.debug.key, buf.debug.value); - } - } - } - - return NULL; -} /**************************************************************************** * MAVLink text message logger @@ -1152,349 +347,6 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) /**************************************************************************** * Public Functions ****************************************************************************/ -void handleMessage(mavlink_message_t *msg) -{ - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { - - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - //check for MAVLINK terminate command - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { - /* This is the link shutdown command, terminate mavlink */ - printf("[mavlink] Terminating .. \n"); - fflush(stdout); - usleep(50000); - - /* terminate other threads and this thread */ - thread_should_exit = true; - - } else { - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - vcmd.command = cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; - - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - } - /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } - } - - if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); - - struct optical_flow_s f; - - f.timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - - /* check if topic is advertised */ - if (flow_pub <= 0) { - flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - } else { - /* publish */ - orb_publish(ORB_ID(optical_flow), flow_pub, &f); - } - } - - if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { - /* Set mode on request */ - mavlink_set_mode_t new_mode; - mavlink_msg_set_mode_decode(msg, &new_mode); - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = MAV_CMD_DO_SET_MODE; - vcmd.target_system = new_mode.target_system; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - } else { - /* create command */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } - - /* Handle Vicon position estimates */ - if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); - - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - - if (mavlink_pubs.vicon_position_pub <= 0) { - mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - } else { - orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position); - } - } - - /* Handle quadrotor motor setpoints */ - - if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - //printf("got message\n"); - //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); - - if (mavlink_system.sysid < 4) { - /* - * rate control mode - defined by MAVLink - */ - - uint8_t ml_mode = 0; - bool ml_armed = false; - -// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) { -// ml_armed = true; -// } - - switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - - break; - case 1: - - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - case 2: - - - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; - } - - offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX; - //offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ; - - if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){ - ml_armed = false; - - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - /* check if topic has to be advertised */ - if (mavlink_pubs.offboard_control_sp_pub <= 0) { - mavlink_pubs.offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - } else { - /* Publish */ - orb_publish(ORB_ID(offboard_control_setpoint), mavlink_pubs.offboard_control_sp_pub, &offboard_control_sp); - } - - // /* change armed status if required */ - // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); - - // bool cmd_generated = false; - - // if (v_status.flag_control_offboard_enabled != cmd_armed) { - // vcmd.param1 = cmd_armed; - // vcmd.param2 = 0; - // vcmd.param3 = 0; - // vcmd.param4 = 0; - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // /* check if input has to be enabled */ - // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || - // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || - // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || - // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { - // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); - // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); - // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); - // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // if (cmd_generated) { - // /* check if topic is advertised */ - // if (cmd_pub <= 0) { - // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - // } else { - // /* create command */ - // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - // } - // } - } - } - - /* - * Only decode hil messages in HIL mode. - * - * The HIL mode is enabled by the HIL bit flag - * in the system mode. Either send a set mode - * COMMAND_LONG message or a SET_MODE message - */ - - // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); - - if (mavlink_hil_enabled) { - - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { - - mavlink_hil_state_t hil_state; - mavlink_msg_hil_state_decode(msg, &hil_state); - - // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " - // "ROLL %i \n PITCH %i \n YAW %i \n" - // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", - // hil_state.lat/1000000, // 1e7 - // hil_state.lon/1000000, // 1e7 - // hil_state.alt/1000, // mm - // hil_state.roll, // float rad - // hil_state.pitch, // float rad - // hil_state.yaw, // float rad - // hil_state.rollspeed, // float rad/s - // hil_state.pitchspeed, // float rad/s - // hil_state.yawspeed); // float rad/s - - - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - 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; - 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.counter++; - hil_attitude.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); - } - - if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - struct rc_channels_s rc_hil; - memset(&rc_hil, 0, sizeof(rc_hil)); - static orb_advert_t rc_pub = 0; - - rc_hil.timestamp = hrt_absolute_time(); - rc_hil.chan_count = 4; - rc_hil.chan[0].raw = 1500 + man.x / 2; - rc_hil.chan[1].raw = 1500 + man.y / 2; - rc_hil.chan[2].raw = 1500 + man.r / 2; - rc_hil.chan[3].raw = 1500 + man.z / 2; - - rc_hil.chan[0].scaled = man.x / 1000.0f; - rc_hil.chan[1].scaled = man.y / 1000.0f; - rc_hil.chan[2].scaled = man.r / 1000.0f; - rc_hil.chan[3].scaled = man.z / 1000.0f; - - struct manual_control_setpoint_s mc; - static orb_advert_t mc_pub = 0; - - mc.timestamp = rc_hil.timestamp; - mc.roll = man.x / 1000.0f; - mc.pitch = man.y / 1000.0f; - mc.yaw = man.r / 1000.0f; - mc.throttle = man.z / 1000.0f; - - /* fake RC channels with manual control input from simulator */ - - - if (rc_pub == 0) { - rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); - } else { - orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); - } - - if (mc_pub == 0) { - mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); - } else { - orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); - } - } - } -} int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { @@ -1593,6 +445,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } +void +mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) +{ + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); +} + void mavlink_update_system(void) { static bool initialized = false; @@ -1631,113 +489,70 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { - wpm = &wpm_s; - - /* initialize global data structs */ - memset(&global_pos, 0, sizeof(global_pos)); - memset(&local_pos, 0, sizeof(local_pos)); - memset(&v_status, 0, sizeof(v_status)); - memset(&rc, 0, sizeof(rc)); - memset(&hil_attitude, 0, sizeof(hil_attitude)); - memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - memset(&vcmd, 0, sizeof(vcmd)); + int ch; + char *device_name = "/dev/ttyS1"; + baudrate = 57600; - /* print welcome text */ - printf("[mavlink] MAVLink v1.0 serial interface starting..\n"); + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; - /* reate the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + switch (ch) { + case 'b': + baudrate = strtoul(optarg, NULL, 10); + if (baudrate == 0) + errx(1, "invalid baud rate '%s'", optarg); + break; - /* default values for arguments */ - char *uart_name = "/dev/ttyS1"; - baudrate = 57600; + case 'd': + device_name = optarg; + break; - /* read program arguments */ - int i; - - for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */ - - if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { - usage(""); - return 0; - } else if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { - if (argc > i + 1) { - uart_name = argv[i + 1]; - i++; - } else { - usage("missing argument for device (-d)"); - return 1; - } - } else if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) { - if (argc > i + 1) { - baudrate = atoi(argv[i + 1]); - i++; - } else { - usage("missing argument for baud rate (-b)"); - return 1; - } - } else if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) { + case 'e': mavlink_link_termination_allowed = true; - } else if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) { + break; + + case 'o': mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; - } else { - usage("out of order or invalid argument"); - return 1; + break; + + default: + usage(); } } struct termios uart_config_original; - bool usb_uart; - uart = mavlink_open_uart(baudrate, uart_name, &uart_config_original, &usb_uart); - - if (uart < 0) { - printf("[mavlink] FAILED to open %s, terminating.\n", uart_name); - goto exit_cleanup; - } + /* print welcome text */ + warnx("MAVLink v1.0 serial interface starting..."); - /* Flush UART */ + /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); - /* Initialize system properties */ - mavlink_update_system(); + /* default values for arguments */ + uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + if (uart < 0) + err(1, "could not open %s", device_name); - /* topics to subscribe globally */ - /* subscribe to ORB for global position */ - mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ - /* subscribe to ORB for local position */ - mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ + /* create the device node that's used for sending text log messages, etc. */ + register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + /* Initialize system properties */ + mavlink_update_system(); - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - pthread_attr_setstacksize(&receiveloop_attr, 2048); - pthread_create(&receive_thread, &receiveloop_attr, receiveloop, &uart); + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); - pthread_attr_t uorb_attr; - pthread_attr_init(&uorb_attr); - /* Set stack size, needs more than 8000 bytes */ - pthread_attr_setstacksize(&uorb_attr, 8192); - pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, &mavlink_subs); + /* start the ORB receiver */ + uorb_receive_thread = uorb_receive_start(); /* initialize waypoint manager */ mavlink_wpm_init(wpm); - uint16_t counter = 0; - - /* make sure all threads have registered their subscriptions */ - while (!mavlink_subs.initialized) { - usleep(500); - } - /* all subscriptions are now active, set up initial guess about rate limits */ - if (baudrate >= 460800) { - /* 200 Hz / 5 ms */ - } else if (baudrate >= 230400) { + if (baudrate >= 230400) { /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); @@ -1787,23 +602,18 @@ int mavlink_thread_main(int argc, char *argv[]) thread_running = true; /* arm counter to go off immediately */ - int lowspeed_counter = 10; + unsigned lowspeed_counter = 10; while (!thread_should_exit) { - /* get local and global position */ - orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); - /* 1 Hz */ if (lowspeed_counter == 10) { mavlink_update_system(); - /* translate the current syste state to mavlink state and mode */ + /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); @@ -1812,10 +622,20 @@ int mavlink_thread_main(int argc, char *argv[]) set_hil_on_off(v_status.flag_hil_enabled); /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, - v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.f, v_status.current_battery * 1000.f, - v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, - v_status.errors_count1, v_status.errors_count2, v_status.errors_count3, v_status.errors_count4); + mavlink_msg_sys_status_send(chan, + v_status.onboard_control_sensors_present, + v_status.onboard_control_sensors_enabled, + v_status.onboard_control_sensors_health, + v_status.load, + v_status.voltage_battery * 1000.0f, + v_status.current_battery * 1000.0f, + v_status.battery_remaining, + v_status.drop_rate_comm, + v_status.errors_comm, + v_status.errors_count1, + v_status.errors_count2, + v_status.errors_count3, + v_status.errors_count4); lowspeed_counter = 0; } lowspeed_counter++; @@ -1831,6 +651,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); + /* sleep quarter the time */ usleep(25000); if (baudrate > 57600) { @@ -1841,9 +662,10 @@ int mavlink_thread_main(int argc, char *argv[]) usleep(10000); /* send one string at 10 Hz */ - mavlink_missionlib_send_gcs_string(mavlink_message_string); - mavlink_message_string[0] = '\0'; - counter++; + if (mavlink_message_string[0] != '\0') { + mavlink_missionlib_send_gcs_string(mavlink_message_string); + mavlink_message_string[0] = '\0'; + } /* sleep 15 ms */ usleep(15000); @@ -1854,54 +676,34 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ - if (!usb_uart) { - int termios_state; - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", uart_name); - } - - printf("[mavlink] Restored original UART config, exiting..\n"); - } - -exit_cleanup: - - /* close uart */ - close(uart); - - /* close subscriptions */ - close(mavlink_subs.global_pos_sub); - close(mavlink_subs.local_pos_sub); - - fflush(stdout); - fflush(stderr); + if (!usb_uart) + tcsetattr(uart, TCSANOW, &uart_config_original); thread_running = false; - return 0; + exit(0); } static void -usage(const char *reason) +usage() { - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: mavlink {start|stop|status} [-d <devicename>] [-b <baudrate>] [-e/--exit-allowed]\n\n"); + fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>] [-e] [-o]\n" + " mavlink stop\n" + " mavlink status\n"); exit(1); } int mavlink_main(int argc, char *argv[]) { + if (argc < 1) - usage("missing command"); + errx(1, "missing command"); if (!strcmp(argv[1], "start")) { - if (thread_running) { - printf("mavlink already running\n"); - /* this is not an error */ - exit(0); - } + /* this is not an error */ + if (thread_running) + errx(0, "mavlink already running\n"); thread_should_exit = false; mavlink_task = task_spawn("mavlink", @@ -1909,25 +711,24 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + argv); exit(0); } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; + /* XXX should wait for it to actually exit here */ exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmavlink app is running\n"); + errx(0, "running"); } else { - printf("\tmavlink app not started\n"); + errx(1, "not running"); } - exit(0); } - usage("unrecognized command"); - exit(1); + errx(1, "unrecognized command"); } diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h index 26bc26fdf..8d34c3924 100644 --- a/apps/mavlink/mavlink_bridge_header.h +++ b/apps/mavlink/mavlink_bridge_header.h @@ -62,28 +62,12 @@ */ extern mavlink_system_t mavlink_system; - -mqd_t gps_queue; -int uart; - - /** * @brief Send multiple chars (uint8_t) over a comm channel * * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 * @param ch Character to send */ -static inline void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, uint16_t length) -{ - ssize_t ret; - - if (chan == MAVLINK_COMM_0) { - ret = write(uart, ch, (size_t)(sizeof(uint8_t) * length)); - - if (ret != length) { - printf("[mavlink] Error: Written %u instead of %u\n", ret, length); - } - } -} +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h new file mode 100644 index 000000000..95790db93 --- /dev/null +++ b/apps/mavlink/mavlink_hil.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_hil.h + * Hardware-in-the-loop simulation support. + */ + +#pragma once + +extern bool mavlink_hil_enabled; + +extern struct vehicle_global_position_s hil_global_pos; +extern struct vehicle_attitude_s hil_attitude; +extern orb_advert_t pub_hil_global_pos; +extern orb_advert_t pub_hil_attitude; + +/** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ +extern int set_hil_on_off(bool hil_enabled);
\ No newline at end of file diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 2e15174bb..8874fe528 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file mavlink_parameters.c * MAVLink parameter protocol implementation (BSD-relicensed). + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include "mavlink_parameters.h" diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h index 950f82d2d..37fd44286 100644 --- a/apps/mavlink/mavlink_parameters.h +++ b/apps/mavlink/mavlink_parameters.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file mavlink_parameters.h * MAVLink parameter protocol definitions (BSD-relicensed). + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ /* This assumes you have the mavlink headers on your include path diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c new file mode 100644 index 000000000..ad46c3ede --- /dev/null +++ b/apps/mavlink/mavlink_receiver.c @@ -0,0 +1,495 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_receiver.c + * MAVLink protocol message receive and dispatch + */ + +/* XXX trim includes */ +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <v1.0/common/mavlink.h> +#include <arch/board/up_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> + +#include "waypoints.h" +#include "mavlink_log.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "mavlink_parameters.h" +#include "util.h" + +/* XXX should be in a header somewhere */ +pthread_t receive_start(int uart); + +static void handle_message(mavlink_message_t *msg); +static void *receive_thread(void *arg); + +static mavlink_status_t status; +static struct vehicle_vicon_position_s vicon_position; +static struct vehicle_command_s vcmd; +static struct offboard_control_setpoint_s offboard_control_sp; + +struct vehicle_global_position_s hil_global_pos; +struct vehicle_attitude_s hil_attitude; +orb_advert_t pub_hil_global_pos = -1; +orb_advert_t pub_hil_attitude = -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; + +static void +handle_message(mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + printf("[mavlink] Terminating .. \n"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + thread_should_exit = true; + + } else { + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } + /* publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + } + + if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + + f.timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + /* check if topic is advertised */ + if (flow_pub <= 0) { + flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &f); + } + } + + if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { + /* Set mode on request */ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = new_mode.custom_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + + /* Handle Vicon position estimates */ + if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); + + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + + if (vicon_position_pub <= 0) { + vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { + orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); + } + } + + /* Handle quadrotor motor setpoints */ + + if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + //printf("got message\n"); + //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); + + if (mavlink_system.sysid < 4) { + /* + * rate control mode - defined by MAVLink + */ + + uint8_t ml_mode = 0; + bool ml_armed = false; + +// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) { +// ml_armed = true; +// } + + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + + break; + case 1: + + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + case 2: + + + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } + + offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX; + //offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ; + + if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){ + ml_armed = false; + + } + + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = ml_mode; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + /* check if topic has to be advertised */ + if (offboard_control_sp_pub <= 0) { + offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { + /* Publish */ + orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); + } + + // /* change armed status if required */ + // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); + + // bool cmd_generated = false; + + // if (v_status.flag_control_offboard_enabled != cmd_armed) { + // vcmd.param1 = cmd_armed; + // vcmd.param2 = 0; + // vcmd.param3 = 0; + // vcmd.param4 = 0; + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // /* check if input has to be enabled */ + // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || + // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || + // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || + // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { + // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); + // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); + // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); + // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // if (cmd_generated) { + // /* check if topic is advertised */ + // if (cmd_pub <= 0) { + // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + // } else { + // /* create command */ + // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + // } + // } + } + } + + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + */ + + // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); + + if (mavlink_hil_enabled) { + + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { + + mavlink_hil_state_t hil_state; + mavlink_msg_hil_state_decode(msg, &hil_state); + + // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " + // "ROLL %i \n PITCH %i \n YAW %i \n" + // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", + // hil_state.lat/1000000, // 1e7 + // hil_state.lon/1000000, // 1e7 + // hil_state.alt/1000, // mm + // hil_state.roll, // float rad + // hil_state.pitch, // float rad + // hil_state.yaw, // float rad + // hil_state.rollspeed, // float rad/s + // hil_state.pitchspeed, // float rad/s + // hil_state.yawspeed); // float rad/s + + + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + 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; + 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.counter++; + hil_attitude.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } + + if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + struct rc_channels_s rc_hil; + memset(&rc_hil, 0, sizeof(rc_hil)); + static orb_advert_t rc_pub = 0; + + rc_hil.timestamp = hrt_absolute_time(); + rc_hil.chan_count = 4; + rc_hil.chan[0].raw = 1500 + man.x / 2; + rc_hil.chan[1].raw = 1500 + man.y / 2; + rc_hil.chan[2].raw = 1500 + man.r / 2; + rc_hil.chan[3].raw = 1500 + man.z / 2; + + rc_hil.chan[0].scaled = man.x / 1000.0f; + rc_hil.chan[1].scaled = man.y / 1000.0f; + rc_hil.chan[2].scaled = man.r / 1000.0f; + rc_hil.chan[3].scaled = man.z / 1000.0f; + + struct manual_control_setpoint_s mc; + static orb_advert_t mc_pub = 0; + + mc.timestamp = rc_hil.timestamp; + mc.roll = man.x / 1000.0f; + mc.pitch = man.y / 1000.0f; + mc.yaw = man.r / 1000.0f; + mc.throttle = man.z / 1000.0f; + + /* fake RC channels with manual control input from simulator */ + + + if (rc_pub == 0) { + rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); + } else { + orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); + } + + if (mc_pub == 0) { + mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); + } else { + orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); + } + } + } +} + + +/** + * Receive data from UART. + */ +static void * +receive_thread(void *arg) +{ + int uart_fd = *((int*)arg); + + const int timeout = 1000; + uint8_t ch; + + mavlink_message_t msg; + + prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + + while (!thread_should_exit) { + + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + + if (poll(fds, 1, timeout) > 0) { + /* non-blocking read until buffer is empty */ + int nread = 0; + + do { + nread = read(uart_fd, &ch, 1); + + if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* handle generic messages and commands */ + handle_message(&msg); + + /* Handle packet with waypoint component */ + mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + + /* Handle packet with parameter component */ + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + } + } while (nread > 0); + } + } + + return NULL; +} + +pthread_t +receive_start(int uart) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + pthread_attr_setstacksize(&receiveloop_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + return thread; +}
\ No newline at end of file diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c new file mode 100644 index 000000000..d2be9a88d --- /dev/null +++ b/apps/mavlink/missionlib.c @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file missionlib.h + * MAVLink missionlib components + */ + +// XXX trim includes +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <v1.0/common/mavlink.h> +#include <arch/board/up_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> + +#include "waypoints.h" +#include "mavlink_log.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" +#include "waypoints.h" +#include "mavlink_parameters.h" + +static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + +int +mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + + mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); + return 0; +} + +int +mavlink_missionlib_send_gcs_string(const char *string) +{ + const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + mavlink_statustext_t statustext; + int i = 0; + + while (i < len - 1) { + statustext.text[i] = string[i]; + + if (string[i] == '\0') + break; + + i++; + } + + if (i > 1) { + /* Enforce null termination */ + statustext.text[i] = '\0'; + mavlink_message_t msg; + + mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + return mavlink_missionlib_send_message(&msg); + } else { + return 1; + } +} + +/** + * Get system time since boot in microseconds + * + * @return the system time since boot in microseconds + */ +uint64_t mavlink_missionlib_get_system_timestamp() +{ + return hrt_absolute_time(); +} + +/** + * This callback is executed each time a waypoint changes. + * + * It publishes the vehicle_global_position_setpoint_s or the + * vehicle_local_position_setpoint_s topic, depending on the type of waypoint + */ +void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + 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 local_position_setpoint_pub = -1; + char buf[50] = {0}; + + /* Update controller setpoints */ + if (frame == (int)MAV_FRAME_GLOBAL) { + /* global, absolute waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = false; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + } + 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) { + /* global, relative alt (in relation to HOME) waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = true; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { + 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) { + /* local, absolute waypoint */ + struct vehicle_local_position_setpoint_s sp; + sp.x = param5_lat_x; + sp.y = param6_lon_y; + sp.z = param7_alt_z; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ + if (local_position_setpoint_pub < 0) { + local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); + } + 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); + } + + 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/apps/mavlink/missionlib.h b/apps/mavlink/missionlib.h new file mode 100644 index 000000000..3439c185d --- /dev/null +++ b/apps/mavlink/missionlib.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file missionlib.h + * MAVLink mission helper library + */ + +#pragma once + +#include <v1.0/common/mavlink.h> + +//extern void mavlink_wpm_send_message(mavlink_message_t *msg); +//extern void mavlink_wpm_send_gcs_string(const char *string); +//extern uint64_t mavlink_wpm_get_system_timestamp(void); +extern int mavlink_missionlib_send_message(mavlink_message_t *msg); +extern int mavlink_missionlib_send_gcs_string(const char *string); +extern uint64_t mavlink_missionlib_get_system_timestamp(void); +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c new file mode 100644 index 000000000..39358922d --- /dev/null +++ b/apps/mavlink/orb_listener.c @@ -0,0 +1,640 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file orb_listener.c + * Monitors ORB topics and sends update messages as appropriate. + */ + +// XXX trim includes +#include <nuttx/config.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <v1.0/common/mavlink.h> +#include <arch/board/up_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <sys/prctl.h> +#include <stdlib.h> +#include <poll.h> + +#include "waypoints.h" +#include "mavlink_log.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" + +struct vehicle_global_position_s global_pos; +struct vehicle_local_position_s local_pos; +struct vehicle_status_s v_status; +struct rc_channels_s rc; +struct actuator_armed_s armed; + +struct mavlink_subscriptions mavlink_subs; + +static int status_sub; +static int rc_sub; + +static unsigned int sensors_raw_counter; +static unsigned int attitude_counter; +static unsigned int gps_counter; + +/* + * Last sensor loop time + * some outputs are better timestamped + * with this "global" reference. + */ +static uint64_t last_sensor_timestamp; + +static void *uorb_receive_thread(void *arg); + +struct listener +{ + void (*callback)(struct listener *l); + int *subp; + uintptr_t arg; +}; + +static void l_sensor_combined(struct listener *l); +static void l_vehicle_attitude(struct listener *l); +static void l_vehicle_gps_position(struct listener *l); +static void l_vehicle_status(struct listener *l); +static void l_rc_channels(struct listener *l); +static void l_global_position(struct listener *l); +static void l_local_position(struct listener *l); +static void l_global_position_setpoint(struct listener *l); +static void l_local_position_setpoint(struct listener *l); +static void l_attitude_setpoint(struct listener *l); +static void l_actuator_outputs(struct listener *l); +static void l_actuator_armed(struct listener *l); +static void l_manual_control_setpoint(struct listener *l); +static void l_vehicle_attitude_controls(struct listener *l); +static void l_debug_key_value(struct listener *l); + +struct listener listeners[] = { + {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, + {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, + {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, + {l_vehicle_status, &status_sub, 0}, + {l_rc_channels, &rc_sub, 0}, + {l_global_position, &mavlink_subs.global_pos_sub, 0}, + {l_local_position, &mavlink_subs.local_pos_sub, 0}, + {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, + {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, + {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, + {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, + {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, + {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, +}; + +static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); + +void +l_sensor_combined(struct listener *l) +{ + struct sensor_combined_s raw; + + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); + + last_sensor_timestamp = raw.timestamp; + + /* send raw imu data */ + // mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, raw.accelerometer_raw[0], + // raw.accelerometer_raw[1], raw.accelerometer_raw[2], raw.gyro_raw[0], + // raw.gyro_raw[1], raw.gyro_raw[2], raw.magnetometer_raw[0], + // raw.magnetometer_raw[1], raw.magnetometer_raw[2]); + + /* mark individual fields as changed */ + uint16_t fields_updated = 0; + static unsigned accel_counter = 0; + static unsigned gyro_counter = 0; + static unsigned mag_counter = 0; + static unsigned baro_counter = 0; + + if (accel_counter != raw.accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = raw.accelerometer_counter; + } + + if (gyro_counter != raw.gyro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = raw.gyro_counter; + } + + if (mag_counter != raw.magnetometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = raw.magnetometer_counter; + } + + if (baro_counter != raw.baro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = raw.baro_counter; + } + + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, + raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], + raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], + 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_alt_meter, raw.baro_temp_celcius, + fields_updated); + /* send pressure */ + //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, raw.timestamp / 1000, raw.baro_pres_mbar, raw.baro_alt_meter, raw.baro_temp_celcius * 100); + + sensors_raw_counter++; +} + +void +l_vehicle_attitude(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); + + /* send sensor values */ + mavlink_msg_attitude_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.roll, + att.pitch, + att.yaw, + att.rollspeed, + att.pitchspeed, + att.yawspeed); + + attitude_counter++; +} + +void +l_vehicle_gps_position(struct listener *l) +{ + struct vehicle_gps_position_s gps; + + /* copy gps data into local buffer */ + orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + + /* GPS position */ + mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, + gps.timestamp, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + gps.eph, + gps.epv, + gps.vel, + gps.cog, + gps.satellites_visible); + + if (gps.satellite_info_available && (gps_counter % 4 == 0)) { + mavlink_msg_gps_status_send(MAVLINK_COMM_0, + gps.satellites_visible, + gps.satellite_prn, + gps.satellite_used, + gps.satellite_elevation, + gps.satellite_azimuth, + gps.satellite_snr); + } + + gps_counter++; +} + +void +l_vehicle_status(struct listener *l) +{ + /* immediately communicate state changes back to user */ + orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + + /* enable or disable HIL */ + set_hil_on_off(v_status.flag_hil_enabled); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_mode, + v_status.state_machine, + mavlink_state); +} + +void +l_rc_channels(struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(chan, + rc.timestamp / 1000, + 0, + rc.chan[0].raw, + rc.chan[1].raw, + rc.chan[2].raw, + rc.chan[3].raw, + rc.chan[4].raw, + rc.chan[5].raw, + rc.chan[6].raw, + rc.chan[7].raw, + rc.rssi); +} + +void +l_global_position(struct listener *l) +{ + /* copy global position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); + + uint64_t timestamp = global_pos.timestamp; + int32_t lat = global_pos.lat; + int32_t lon = global_pos.lon; + int32_t alt = (int32_t)(global_pos.alt*1000); + int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); + int16_t vx = (int16_t)(global_pos.vx * 100.0f); + int16_t vy = (int16_t)(global_pos.vy * 100.0f); + int16_t vz = (int16_t)(global_pos.vz * 100.0f); + + /* heading in degrees * 10, from 0 to 36.000) */ + uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + + mavlink_msg_global_position_int_send(MAVLINK_COMM_0, + timestamp / 1000, + lat, + lon, + alt, + relative_alt, + vx, + vy, + vz, + hdg); +} + +void +l_local_position(struct listener *l) +{ + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); + + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, + local_pos.timestamp / 1000, + local_pos.x, + local_pos.y, + local_pos.z, + local_pos.vx, + local_pos.vy, + local_pos.vz); +} + +void +l_global_position_setpoint(struct listener *l) +{ + struct vehicle_global_position_setpoint_s global_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); + + uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + if (global_sp.altitude_is_relative) + coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + + mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + coordinate_frame, + global_sp.lat, + global_sp.lon, + global_sp.altitude, + global_sp.yaw); +} + +void +l_local_position_setpoint(struct listener *l) +{ + struct vehicle_local_position_setpoint_s local_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); + + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, + MAV_FRAME_LOCAL_NED, + local_sp.x, + local_sp.y, + local_sp.z, + local_sp.yaw); +} + +void +l_attitude_setpoint(struct listener *l) +{ + struct vehicle_attitude_setpoint_s att_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); + + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, + att_sp.timestamp/1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); +} + +void +l_actuator_outputs(struct listener *l) +{ + struct actuator_outputs_s act_outputs; + + orb_id_t ids[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + /* copy actuator data into local buffer */ + orb_copy(ids[l->arg], *l->subp, &act_outputs); + + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, + l->arg /* port number */, + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7]); +} + +void +l_actuator_armed(struct listener *l) +{ + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); +} + +void +l_manual_control_setpoint(struct listener *l) +{ + struct manual_control_setpoint_s man_control; + + /* copy manual control data into local buffer */ + orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); + + mavlink_msg_manual_control_send(MAVLINK_COMM_0, + mavlink_system.sysid, + man_control.roll * 1000, + man_control.pitch * 1000, + man_control.yaw * 1000, + man_control.throttle * 1000, + 0); +} + +void +l_vehicle_attitude_controls(struct listener *l) +{ + struct actuator_controls_s actuators; + + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + + /* 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, + "ctrl0 ", + actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators.control[3]); + + /* Only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + actuators.control[0], + actuators.control[1], + actuators.control[2], + actuators.control[3], + 0, + 0, + 0, + 0, + mavlink_mode, + 0); + } +} + +void +l_debug_key_value(struct listener *l) +{ + struct debug_key_value_s debug; + + orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); + + /* Enforce null termination */ + debug.key[sizeof(debug.key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + debug.key, + debug.value); +} + +static void * +uorb_receive_thread(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + const int timeout = 1000; + + /* + * Initialise listener array. + * + * Might want to invoke each listener once to set initial state. + */ + struct pollfd fds[n_listeners]; + for (unsigned i = 0; i < n_listeners; i++) { + fds[i].fd = *listeners[i].subp; + + /* Invoke callback to set initial state */ + //listeners[i].callback(&listener[i]); + } + + while (!thread_should_exit) { + + int poll_ret = poll(fds, n_listeners, timeout); + + /* handle the poll result */ + if (poll_ret == 0) { + mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + } else if (poll_ret < 0) { + mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + } else { + + for (unsigned i = 0; i < n_listeners; i++) { + if (fds[i].revents & POLLIN) + listeners[i].callback(&listeners[i]); + } + } + } + + return NULL; +} + +pthread_t +uorb_receive_start(void) +{ + /* --- SENSORS RAW VALUE --- */ + mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.sensor_sub, 100); /* 10Hz updates */ + + /* --- ATTITUDE VALUE --- */ + mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.att_sub, 100); /* 10Hz updates */ + + /* --- GPS VALUE --- */ + mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + + /* --- SYSTEM STATE --- */ + status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + + /* --- RC CHANNELS VALUE --- */ + rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(rc_sub, 100); /* 10Hz updates */ + + /* --- GLOBAL POS VALUE --- */ + mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ + + /* --- LOCAL POS VALUE --- */ + mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ + + /* --- GLOBAL SETPOINT VALUE --- */ + mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ + + /* --- LOCAL SETPOINT VALUE --- */ + mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ + + /* --- ATTITUDE SETPOINT VALUE --- */ + mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + + /* --- ACTUATOR OUTPUTS --- */ + mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR ARMED VALUE --- */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + + /* --- MAPPED MANUAL CONTROL INPUTS --- */ + mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR CONTROL VALUE --- */ + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ + + /* --- DEBUG VALUE OUTPUT --- */ + mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ + + /* start the listener loop */ + pthread_attr_t uorb_attr; + pthread_attr_init(&uorb_attr); + + /* Set stack size, needs more than 8000 bytes */ + /* XXX verify, should not need anything like this much unless MAVLink really sucks */ + pthread_attr_setstacksize(&uorb_attr, 8192); + + pthread_t thread; + pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + return thread; +} diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h new file mode 100644 index 000000000..f041e7c4c --- /dev/null +++ b/apps/mavlink/orb_topics.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file orb_topics.h + * Common sets of topics subscribed to or published by the MAVLink driver, + * and structures maintained by those subscriptions. + */ +#pragma once + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_command.h> +#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_attitude_setpoint.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/debug_key_value.h> + +struct mavlink_subscriptions { + int sensor_sub; + int att_sub; + int global_pos_sub; + int act_0_sub; + int act_1_sub; + int act_2_sub; + int act_3_sub; + int gps_sub; + int man_control_sp_sub; + int armed_sub; + int actuators_sub; + int local_pos_sub; + int spa_sub; + int spl_sub; + int spg_sub; + int debug_key_value; +}; + +extern struct mavlink_subscriptions mavlink_subs; + +/** Global position */ +extern struct vehicle_global_position_s global_pos; + +/** Local position */ +extern struct vehicle_local_position_s local_pos; + +/** Vehicle status */ +extern struct vehicle_status_s v_status; + +/** RC channels */ +extern struct rc_channels_s rc; + +/** Actuator armed state */ +extern struct actuator_armed_s armed; + +/** Worker thread starter */ +extern pthread_t uorb_receive_start(void); diff --git a/apps/mavlink/util.h b/apps/mavlink/util.h new file mode 100644 index 000000000..a4ff06a88 --- /dev/null +++ b/apps/mavlink/util.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file util.h + * Utility and helper functions and data. + */ + +#pragma once + +/** MAVLink communications channel */ +extern uint8_t chan; + +/** Shutdown marker */ +extern volatile bool thread_should_exit; + +/** Waypoint storage */ +extern mavlink_wpm_storage *wpm; + +/** + * Translate the custom state into standard mavlink modes and state. + */ +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 8ed80c817..3d9e5750a 100644 --- a/apps/mavlink/waypoints.c +++ b/apps/mavlink/waypoints.c @@ -40,12 +40,15 @@ * MAVLink waypoint protocol implementation (BSD-relicensed). */ -#include "waypoints.h" #include <math.h> #include <sys/prctl.h> #include <unistd.h> #include <stdio.h> +#include "missionlib.h" +#include "waypoints.h" +#include "util.h" + #ifndef FM_PI #define FM_PI 3.1415926535897932384626433832795f #endif @@ -53,15 +56,6 @@ bool debug = false; bool verbose = false; -extern mavlink_wpm_storage *wpm; - -extern void mavlink_missionlib_send_message(mavlink_message_t *msg); -extern void mavlink_missionlib_send_gcs_string(const char *string); -extern uint64_t mavlink_missionlib_get_system_timestamp(void); -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - #define MAVLINK_WPM_NO_PRINTF diff --git a/apps/mavlink/waypoints.h b/apps/mavlink/waypoints.h index c525f4553..736d1f119 100644 --- a/apps/mavlink/waypoints.h +++ b/apps/mavlink/waypoints.h @@ -46,7 +46,6 @@ or in the same folder as this source file */ #include <v1.0/mavlink_types.h> -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *buffer, uint16_t len); #ifndef MAVLINK_SEND_UART_BYTES #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index c02cf2fcf..36225386c 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -144,7 +144,6 @@ int sdlog_main(int argc, char *argv[]) 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; exit(0); } @@ -215,7 +214,7 @@ int sdlog_thread_main(int argc, char *argv[]) { char folder_path[64]; if (create_logfolder(folder_path)) - errx(1, "unable to create logging folder, exiting"); + errx(1, "unable to create logging folder, exiting."); /* create sensorfile */ int sensorfile = -1; diff --git a/apps/systemlib/getopt_long.h b/apps/systemlib/getopt_long.h index 3e51550a6..61e8e76f3 100644 --- a/apps/systemlib/getopt_long.h +++ b/apps/systemlib/getopt_long.h @@ -124,9 +124,9 @@ extern "C" #if 0 int getopt (int argc, char **argv, char *optstring); #endif - int getopt_long (int argc, char **argv, const char *shortopts, + __EXPORT int getopt_long (int argc, char **argv, const char *shortopts, const GETOPT_LONG_OPTION_T * longopts, int *longind); - int getopt_long_only (int argc, char **argv, const char *shortopts, + __EXPORT int getopt_long_only (int argc, char **argv, const char *shortopts, const GETOPT_LONG_OPTION_T * longopts, int *longind); #ifdef __cplusplus diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h index 172c37d0f..82ff46ad2 100644 --- a/apps/uORB/uORB.h +++ b/apps/uORB/uORB.h @@ -54,6 +54,8 @@ struct orb_metadata { const size_t o_size; /**< object size */ }; +typedef const struct orb_metadata *orb_id_t; + /** * Generates a pointer to the uORB metadata structure for * a given topic. |