diff --git a/.gitignore b/.gitignore index b5bd27d539..ea416fae48 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,5 @@ .built -.context +*.context .depend .config .version @@ -10,6 +10,7 @@ apps/namedapp/namedapp_proto.h Make.dep *.o *.a +*~ Images/*.bin Images/*.px4 nuttx/Make.defs diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 3377abe77a..409bde33b2 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -20,6 +20,11 @@ set MODE autostart set USB autoconnect +# +# Start playing the startup tune +# +tone_alarm start + # # Try to mount the microSD card. # @@ -31,9 +36,6 @@ else echo "[init] no microSD card found" fi -usleep 500 - - # # Look for an init script on the microSD card. # diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d4a473b66..833425aa62 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -286,7 +286,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* close uarts */ close(ardrone_write); - //ar_multiplexing_deinit(gpios); /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(device, &uart_config_original); diff --git a/apps/attitude_estimator_ekf/.context b/apps/attitude_estimator_ekf/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index e5620138a5..4d531867ce 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -40,6 +40,7 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ + codegen/rdivide.c \ codegen/attitudeKalmanfilter_initialize.c \ codegen/attitudeKalmanfilter_terminate.c \ codegen/rt_nonfinite.c \ @@ -47,8 +48,9 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rtGetNaN.c \ codegen/norm.c \ codegen/diag.c \ - codegen/cross.c \ - codegen/power.c + codegen/power.c \ + codegen/cross.c + # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m old mode 100755 new mode 100644 diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj old mode 100755 new mode 100644 diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 1d4df87fe3..7bb8490e52 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -71,46 +71,34 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds -static float dt = 1.0f; +static float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ -static float z_k[9]; /**< Measurement vector */ -static float x_aposteriori_k[12]; /**< */ -static float x_aposteriori[12]; +static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ +static float x_aposteriori_k[12]; /**< states */ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, +}; /**< init: diagonal matrix with big values */ -static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ +static float x_aposteriori[12]; +static float P_aposteriori[144]; /* output euler angles */ static float euler[3] = {0.0f, 0.0f, 0.0f}; static float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ + 0, 1.f, 0, + 0, 0, 1.f +}; /**< init: identity matrix */ static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -159,11 +147,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) thread_should_exit = false; attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 20000, - attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 20000, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -236,7 +224,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* advertise debug value */ struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + orb_advert_t pub_dbg = -1; /* keep track of sensor updates */ uint32_t sensor_last_count[3] = {0, 0, 0}; @@ -250,12 +238,18 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* initialize parameter handles */ parameters_init(&ekf_param_handles); + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + /* Main loop*/ while (!thread_should_exit) { struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } + { .fd = sub_raw, .events = POLLIN }, + { .fd = sub_params, .events = POLLIN } }; int ret = poll(fds, 2, 1000); @@ -265,7 +259,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* XXX this means no sensor data - should be critical or emergency */ printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); } else { - + /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -282,156 +276,178 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; + if (!initialized) { - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + offset_count++; - z_k[0] = raw.gyro_rad_s[0]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[2]; + if (hrt_absolute_time() - start_time > 3000000LL) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + } + } else { - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { - update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; - } - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; - /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { - update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; - } - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; + z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + int32_t z_k_sizes = 9; + // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05 && dt > 0.005) + { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + uint64_t timing_start = hrt_absolute_time(); + + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + + uint64_t timing_diff = hrt_absolute_time() - timing_start; + + // /* print rotation matrix every 200th time */ + if (printcounter % 200 == 0) { + // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", + // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], + // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], + // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); + + + // } + + //printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + //printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); + //printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]); + // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + } + + // int i = printcounter % 9; + + // // for (int i = 0; i < 9; i++) { + // char name[10]; + // sprintf(name, "xapo #%d", i); + // memcpy(dbg.key, name, sizeof(dbg.key)); + // dbg.value = x_aposteriori[i]; + // if (pub_dbg < 0) { + // pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + // } else { + // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); // } - overloadcounter++; + printcounter++; + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2]; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + //att.yawspeed =z_k[2] ; + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } - - int32_t z_k_sizes = 9; - // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - - static bool const_initialized = false; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) - { - dt = 0.005f; - parameters_update(&ekf_param_handles, &ekf_params); - - x_aposteriori_k[0] = z_k[0]; - x_aposteriori_k[1] = z_k[1]; - x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = 0.0f; - x_aposteriori_k[4] = 0.0f; - x_aposteriori_k[5] = 0.0f; - x_aposteriori_k[6] = z_k[3]; - x_aposteriori_k[7] = z_k[4]; - x_aposteriori_k[8] = z_k[5]; - x_aposteriori_k[9] = z_k[6]; - x_aposteriori_k[10] = z_k[7]; - x_aposteriori_k[11] = z_k[8]; - - const_initialized = true; - } - - /* do not execute the filter if not initialized */ - if (!const_initialized) { - continue; - } - - dt = 0.004f; - - uint64_t timing_start = hrt_absolute_time(); - // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, - // Rot_matrix, x_aposteriori, P_aposteriori); - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); - /* swap values for next iteration */ - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); - uint64_t timing_diff = hrt_absolute_time() - timing_start; - - // /* print rotation matrix every 200th time */ - if (printcounter % 200 == 0) { - // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", - // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], - // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], - // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); - - - // } - - //printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - //printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); - //printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]); - // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), - // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), - // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); - } - - // int i = printcounter % 9; - - // // for (int i = 0; i < 9; i++) { - // char name[10]; - // sprintf(name, "xapo #%d", i); - // memcpy(dbg.key, name, sizeof(dbg.key)); - // dbg.value = x_aposteriori[i]; - // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - - printcounter++; - - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2]; - - // XXX replace with x_apo after fix to filter - att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0]; - att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1]; - att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2]; - - /* copy offsets */ - memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); - att.R_valid = true; - - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } } diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index d95c56368b..4fc2e0452a 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,13 +61,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); +PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); +PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f); /* magnetometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c old mode 100755 new mode 100644 index 86d872fd2c..04f6ea267e --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,13 +3,14 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ /* Include files */ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" +#include "rdivide.h" #include "norm.h" #include "cross.h" #include "eye.h" @@ -32,10 +33,24 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) { real32_T y; + int32_T b_u0; + int32_T b_u1; if (rtIsNaNF(u0) || rtIsNaNF(u1)) { y = ((real32_T)rtNaN); } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); + if (u0 > 0.0F) { + b_u0 = 1; + } else { + b_u0 = -1; + } + + if (u1 > 0.0F) { + b_u1 = 1; + } else { + b_u1 = -1; + } + + y = (real32_T)atan2f((real32_T)b_u0, (real32_T)b_u1); } else if (u1 == 0.0F) { if (u0 > 0.0F) { y = RT_PIF / 2.0F; @@ -61,76 +76,75 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const P_aposteriori[144]) { real32_T a[12]; - int32_T i; real32_T b_a[12]; + int32_T i; real32_T Q[144]; real32_T O[9]; real_T dv0[9]; real32_T c_a[9]; real32_T d_a[9]; real32_T x_n_b[3]; + real32_T b_x_aposteriori_k[3]; + real32_T m_n_b[3]; real32_T z_n_b[3]; real32_T x_apriori[12]; - real32_T y_n_b[3]; int32_T i0; - real32_T e_a[3]; real_T dv1[144]; real32_T A_lin[144]; real32_T b_A_lin[144]; int32_T i1; - real32_T y; real32_T P_apriori[144]; + real32_T f0; real32_T R[81]; real32_T b_P_apriori[108]; - static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + static const int8_T iv0[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T K_k[108]; static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T fv0[81]; real32_T c_P_apriori[36]; - static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 }; + static const int8_T iv2[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T fv1[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T S_k[36]; real32_T d_P_apriori[72]; - static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + static const int8_T iv4[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; real32_T b_K_k[72]; static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T b_r[6]; static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 }; - static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T fv2[6]; real32_T b_z[6]; - real32_T b_y; /* Extended Attitude Kalmanfilter */ /* */ @@ -147,31 +161,43 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* udpIndVect=find(updVect); */ /* process and measurement noise covariance matrix */ /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */ - power(q, 2.0, a); + power(q, a); for (i = 0; i < 12; i++) { b_a[i] = a[i] * dt; } diag(b_a, Q); + /* Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ + /* 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ + /* 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:37' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:38' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:39' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:41' wox= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:42' woy= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:43' woz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:45' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:46' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:47' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:49' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:50' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:51' muz= x_aposteriori_k(12); */ - /* 'attitudeKalmanfilter:54' wk =[wx; */ - /* 'attitudeKalmanfilter:55' wy; */ - /* 'attitudeKalmanfilter:56' wz]; */ - /* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */ - /* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + /* 'attitudeKalmanfilter:44' wx= x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:45' wy= x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:46' wz= x_aposteriori_k(3); */ + /* 'attitudeKalmanfilter:48' wox= x_aposteriori_k(4); */ + /* 'attitudeKalmanfilter:49' woy= x_aposteriori_k(5); */ + /* 'attitudeKalmanfilter:50' woz= x_aposteriori_k(6); */ + /* 'attitudeKalmanfilter:52' zex= x_aposteriori_k(7); */ + /* 'attitudeKalmanfilter:53' zey= x_aposteriori_k(8); */ + /* 'attitudeKalmanfilter:54' zez= x_aposteriori_k(9); */ + /* 'attitudeKalmanfilter:56' mux= x_aposteriori_k(10); */ + /* 'attitudeKalmanfilter:57' muy= x_aposteriori_k(11); */ + /* 'attitudeKalmanfilter:58' muz= x_aposteriori_k(12); */ + /* 'attitudeKalmanfilter:61' wk =[wx; */ + /* 'attitudeKalmanfilter:62' wy; */ + /* 'attitudeKalmanfilter:63' wz]; */ + /* 'attitudeKalmanfilter:65' wok =[wox;woy;woz]; */ + /* 'attitudeKalmanfilter:66' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ O[0] = 0.0F; O[1] = -x_aposteriori_k[2]; O[2] = x_aposteriori_k[1]; @@ -182,33 +208,36 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const O[7] = x_aposteriori_k[0]; O[8] = 0.0F; - /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ + /* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); for (i = 0; i < 9; i++) { c_a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ + /* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); for (i = 0; i < 9; i++) { d_a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:63' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:64' -zez,0,zex; */ - /* 'attitudeKalmanfilter:65' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:67' -muz,0,mux; */ - /* 'attitudeKalmanfilter:68' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:72' E=eye(3); */ - /* 'attitudeKalmanfilter:73' Z=zeros(3); */ - /* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */ + /* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */ + /* 'attitudeKalmanfilter:71' -zez,0,zex; */ + /* 'attitudeKalmanfilter:72' zey,-zex,0]'; */ + /* 'attitudeKalmanfilter:73' MA=[0,muz,-muy; */ + /* 'attitudeKalmanfilter:74' -muz,0,mux; */ + /* 'attitudeKalmanfilter:75' zey,-mux,0]'; */ + /* 'attitudeKalmanfilter:79' E=eye(3); */ + /* 'attitudeKalmanfilter:80' Es=[1,0,0; */ + /* 'attitudeKalmanfilter:81' 0,1,0; */ + /* 'attitudeKalmanfilter:82' 0,0,0]; */ + /* 'attitudeKalmanfilter:83' Z=zeros(3); */ + /* 'attitudeKalmanfilter:84' x_apriori=[wk;wok;zek;muk]; */ x_n_b[0] = x_aposteriori_k[6]; x_n_b[1] = x_aposteriori_k[7]; x_n_b[2] = x_aposteriori_k[8]; - z_n_b[0] = x_aposteriori_k[9]; - z_n_b[1] = x_aposteriori_k[10]; - z_n_b[2] = x_aposteriori_k[11]; + b_x_aposteriori_k[0] = x_aposteriori_k[9]; + b_x_aposteriori_k[1] = x_aposteriori_k[10]; + b_x_aposteriori_k[2] = x_aposteriori_k[11]; x_apriori[0] = x_aposteriori_k[0]; x_apriori[1] = x_aposteriori_k[1]; x_apriori[2] = x_aposteriori_k[2]; @@ -216,28 +245,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const x_apriori[4] = x_aposteriori_k[4]; x_apriori[5] = x_aposteriori_k[5]; for (i = 0; i < 3; i++) { - y_n_b[i] = 0.0F; + m_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; + m_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; } - e_a[i] = 0.0F; + z_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - e_a[i] += d_a[i + 3 * i0] * z_n_b[i0]; + z_n_b[i] += d_a[i + 3 * i0] * b_x_aposteriori_k[i0]; } - x_apriori[i + 6] = y_n_b[i]; + x_apriori[i + 6] = m_n_b[i]; } for (i = 0; i < 3; i++) { - x_apriori[i + 9] = e_a[i]; + x_apriori[i + 9] = z_n_b[i]; } - /* 'attitudeKalmanfilter:76' A_lin=[ Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:77' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:78' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:79' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */ + /* 'attitudeKalmanfilter:86' A_lin=[ Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:87' Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:88' EZ, Z, O, Z */ + /* 'attitudeKalmanfilter:89' MA, Z, Z, O]; */ + /* 'attitudeKalmanfilter:92' A_lin=eye(12)+A_lin*dt; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { @@ -310,7 +339,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } - /* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + /* 'attitudeKalmanfilter:98' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { A_lin[i + 12 * i0] = 0.0F; @@ -323,28 +352,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } - P_apriori[i + 12 * i0] = y + Q[i + 12 * i0]; + P_apriori[i + 12 * i0] = f0 + Q[i + 12 * i0]; } } /* %update */ - /* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ + /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:93' R=diag(r); */ + /* 'attitudeKalmanfilter:103' R=diag(r); */ b_diag(r, R); /* observation matrix */ - /* 'attitudeKalmanfilter:96' H_k=[ E, E, Z, Z; */ - /* 'attitudeKalmanfilter:97' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:98' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */ + /* 'attitudeKalmanfilter:106' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:107' Z, Z, E, Z; */ + /* 'attitudeKalmanfilter:108' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */ + /* 'attitudeKalmanfilter:112' S_k=H_k*P_apriori*H_k'+R; */ + /* 'attitudeKalmanfilter:113' K_k=(P_apriori*H_k'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 9; i0++) { b_P_apriori[i + 12 * i0] = 0.0F; @@ -366,46 +395,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 9; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; + f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; } - fv0[i + 9 * i0] = y + R[i + 9 * i0]; + fv0[i + 9 * i0] = f0 + R[i + 9 * i0]; } } mrdivide(b_P_apriori, fv0, K_k); - /* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; + f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; } - c_a[i] = z[i] - y; + c_a[i] = z[i] - f0; } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 9; i0++) { - y += K_k[i + 12 * i0] * c_a[i0]; + f0 += K_k[i + 12 * i0] * c_a[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ + /* 'attitudeKalmanfilter:117' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 9; i1++) { - y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; + f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -418,17 +447,17 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } } else { - /* 'attitudeKalmanfilter:108' else */ - /* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:118' else */ + /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */ + /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */ c_diag(*(real32_T (*)[3])&r[0], O); /* observation matrix */ - /* 'attitudeKalmanfilter:113' H_k=[ E, E, Z, Z]; */ - /* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:123' H_k=[ E, Z, Z, Z]; */ + /* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { c_P_apriori[i + 12 * i0] = 0.0F; @@ -451,46 +480,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; + f0 += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; } - c_a[i + 3 * i0] = y + O[i + 3 * i0]; + c_a[i + 3 * i0] = f0 + O[i + 3 * i0]; } } b_mrdivide(c_P_apriori, c_a, S_k); - /* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; + f0 += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; } - x_n_b[i] = z[i] - y; + x_n_b[i] = z[i] - f0; } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 3; i0++) { - y += S_k[i + 12 * i0] * x_n_b[i0]; + f0 += S_k[i + 12 * i0] * x_n_b[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:132' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 3; i1++) { - y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; + f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -504,19 +533,19 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } } else { - /* 'attitudeKalmanfilter:123' else */ - /* 'attitudeKalmanfilter:124' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:133' else */ + /* 'attitudeKalmanfilter:134' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */ + /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */ d_diag(*(real32_T (*)[6])&r[0], S_k); /* observation matrix */ - /* 'attitudeKalmanfilter:128' H_k=[ E, E, Z, Z; */ - /* 'attitudeKalmanfilter:129' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:138' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:139' Z, Z, E, Z]; */ + /* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:144' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; @@ -539,46 +568,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; + f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; } - fv1[i + 6 * i0] = y + S_k[i + 6 * i0]; + fv1[i + 6 * i0] = f0 + S_k[i + 6 * i0]; } } c_mrdivide(d_P_apriori, fv1, b_K_k); - /* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; + f0 += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; } - b_r[i] = z[i] - y; + b_r[i] = z[i] - f0; } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - y += b_K_k[i + 12 * i0] * b_r[i0]; + f0 += b_K_k[i + 12 * i0] * b_r[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:148' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; + f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -592,16 +621,16 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } } else { - /* 'attitudeKalmanfilter:139' else */ - /* 'attitudeKalmanfilter:140' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ + /* 'attitudeKalmanfilter:149' else */ + /* 'attitudeKalmanfilter:150' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */ + /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */ /* observation matrix */ - /* 'attitudeKalmanfilter:144' H_k=[ E, E, Z, Z; */ - /* 'attitudeKalmanfilter:145' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:154' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:155' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { b_K_k[i + 6 * i0] = 0.0F; @@ -619,16 +648,16 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; + f0 += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; } - S_k[i + 6 * i0] = y + b_r[3 * (i + i0)]; + S_k[i + 6 * i0] = f0 + b_r[3 * (i + i0)]; } } - /* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:160' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; @@ -641,7 +670,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const c_mrdivide(d_P_apriori, S_k, b_K_k); - /* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:163' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { b_r[i] = z[i]; } @@ -660,24 +689,24 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - y += b_K_k[i + 12 * i0] * b_z[i0]; + f0 += b_K_k[i + 12 * i0] * b_z[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:164' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; + f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -691,57 +720,66 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } } else { - /* 'attitudeKalmanfilter:155' else */ - /* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */ + /* 'attitudeKalmanfilter:165' else */ + /* 'attitudeKalmanfilter:166' x_aposteriori=x_apriori; */ for (i = 0; i < 12; i++) { x_aposteriori[i] = x_apriori[i]; } - /* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */ - memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof - (real32_T)); + /* 'attitudeKalmanfilter:167' P_aposteriori=P_apriori; */ + memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); } } } } /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - y = norm(*(real32_T (*)[3])&x_aposteriori[6]); - - /* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ - b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]); - - /* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */ + /* 'attitudeKalmanfilter:176' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ for (i = 0; i < 3; i++) { - z_n_b[i] = -x_aposteriori[i + 6] / y; - x_n_b[i] = x_aposteriori[i + 9] / b_y; + x_n_b[i] = -x_aposteriori[i + 6]; } - cross(z_n_b, x_n_b, y_n_b); + rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - /* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */ - y = norm(y_n_b); + /* 'attitudeKalmanfilter:177' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ + rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& + x_aposteriori[9]), m_n_b); + + /* 'attitudeKalmanfilter:179' y_n_b=cross(z_n_b,m_n_b); */ for (i = 0; i < 3; i++) { - y_n_b[i] /= y; + x_n_b[i] = m_n_b[i]; } - /* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(y_n_b, z_n_b, x_n_b); + cross(z_n_b, x_n_b, m_n_b); - /* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */ - y = norm(x_n_b); + /* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_n_b); */ for (i = 0; i < 3; i++) { - /* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - Rot_matrix[i] = x_n_b[i] / y; - Rot_matrix[3 + i] = y_n_b[i]; + x_n_b[i] = m_n_b[i]; + } + + rdivide(x_n_b, norm(m_n_b), m_n_b); + + /* 'attitudeKalmanfilter:182' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(m_n_b, z_n_b, x_n_b); + + /* 'attitudeKalmanfilter:183' x_n_b=x_n_b./norm(x_n_b); */ + for (i = 0; i < 3; i++) { + b_x_aposteriori_k[i] = x_n_b[i]; + } + + rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); + + /* 'attitudeKalmanfilter:189' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + for (i = 0; i < 3; i++) { + Rot_matrix[i] = x_n_b[i]; + Rot_matrix[3 + i] = m_n_b[i]; Rot_matrix[6 + i] = z_n_b[i]; } - /* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */ + /* 'attitudeKalmanfilter:193' phi=atan2f(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:194' theta=-asinf(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:195' psi=atan2f(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h old mode 100755 new mode 100644 index f8f99fa5a3..c93d7f4bbc --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __ATTITUDEKALMANFILTER_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c old mode 100755 new mode 100644 index 689bc49e94..67b6fa4229 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h old mode 100755 new mode 100644 index dcce3cd728..610924d757 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __ATTITUDEKALMANFILTER_INITIALIZE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c old mode 100755 new mode 100644 index 39f8f648c8..c896027237 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h old mode 100755 new mode 100644 index ea7b9e03ea..a196307759 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __ATTITUDEKALMANFILTER_TERMINATE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h old mode 100755 new mode 100644 index 6d5704a7a8..eba83d0d99 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c old mode 100755 new mode 100644 index 27da4b6b93..42cd709715 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h old mode 100755 new mode 100644 index 8ef2c475c2..c90d6b3a5c --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __CROSS_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c old mode 100755 new mode 100644 index 81626589d2..7c28e873a3 --- a/apps/attitude_estimator_ekf/codegen/diag.c +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -30,7 +30,7 @@ void b_diag(const real32_T v[9], real32_T d[81]) { int32_T j; - memset((void *)&d[0], 0, 81U * sizeof(real32_T)); + memset(&d[0], 0, 81U * sizeof(real32_T)); for (j = 0; j < 9; j++) { d[j + 9 * j] = v[j]; } @@ -57,7 +57,7 @@ void c_diag(const real32_T v[3], real32_T d[9]) void d_diag(const real32_T v[6], real32_T d[36]) { int32_T j; - memset((void *)&d[0], 0, 36U * sizeof(real32_T)); + memset(&d[0], 0, 36U * sizeof(real32_T)); for (j = 0; j < 6; j++) { d[j + 6 * j] = v[j]; } @@ -69,7 +69,7 @@ void d_diag(const real32_T v[6], real32_T d[36]) void diag(const real32_T v[12], real32_T d[144]) { int32_T j; - memset((void *)&d[0], 0, 144U * sizeof(real32_T)); + memset(&d[0], 0, 144U * sizeof(real32_T)); for (j = 0; j < 12; j++) { d[j + 12 * j] = v[j]; } diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h old mode 100755 new mode 100644 index 10cea81b24..98b411c2d1 --- a/apps/attitude_estimator_ekf/codegen/diag.h +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __DIAG_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c old mode 100755 new mode 100644 index 2db070e6fe..dbd44c6a82 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -30,7 +30,7 @@ void b_eye(real_T I[144]) { int32_T i; - memset((void *)&I[0], 0, 144U * sizeof(real_T)); + memset(&I[0], 0, 144U * sizeof(real_T)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1.0; } @@ -42,7 +42,7 @@ void b_eye(real_T I[144]) void eye(real_T I[9]) { int32_T i; - memset((void *)&I[0], 0, 9U * sizeof(real_T)); + memset(&I[0], 0, 9U * sizeof(real_T)); for (i = 0; i < 3; i++) { I[i + 3 * i] = 1.0; } diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h old mode 100755 new mode 100644 index 027db1c065..325fd23ec2 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __EYE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c old mode 100755 new mode 100644 index ce29e42cd0..0a540775a0 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -29,9 +29,9 @@ */ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) { + real32_T b_A[9]; int32_T rtemp; int32_T k; - real32_T b_A[9]; real32_T b_B[36]; int32_T r1; int32_T r2; @@ -54,15 +54,15 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) r1 = 0; r2 = 1; r3 = 2; - maxval = (real32_T)fabsf(b_A[0]); - a21 = (real32_T)fabsf(b_A[1]); + maxval = (real32_T)fabs(b_A[0]); + a21 = (real32_T)fabs(b_A[1]); if (a21 > maxval) { maxval = a21; r1 = 1; r2 = 0; } - if ((real32_T)fabsf(b_A[2]) > maxval) { + if ((real32_T)fabs(b_A[2]) > maxval) { r1 = 2; r2 = 1; r3 = 0; @@ -74,7 +74,7 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; - if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) { + if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) { rtemp = r2; r2 = r3; r3 = rtemp; @@ -107,51 +107,46 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) */ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) { - int32_T jy; - int32_T iy; real32_T b_A[36]; int8_T ipiv[6]; + int32_T i3; + int32_T iy; int32_T j; - int32_T mmj; - int32_T jj; - int32_T jp1j; int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T loop_ub; + int32_T jy; + int32_T ijA; real32_T Y[72]; - for (jy = 0; jy < 6; jy++) { + for (i3 = 0; i3 < 6; i3++) { for (iy = 0; iy < 6; iy++) { - b_A[iy + 6 * jy] = B[jy + 6 * iy]; + b_A[iy + 6 * i3] = B[i3 + 6 * iy]; } - ipiv[jy] = (int8_T)(1 + jy); + ipiv[i3] = (int8_T)(1 + i3); } for (j = 0; j < 5; j++) { - mmj = -j; - jj = j * 7; - jp1j = jj + 1; - c = mmj + 6; - jy = 0; - ix = jj; - temp = (real32_T)fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { + c = j * 7; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 6 - j; k++) { ix++; - s = (real32_T)fabsf(b_A[ix]); + s = (real32_T)fabs(b_A[ix]); if (s > temp) { - jy = k - 1; + iy = k - 1; temp = s; } } - if ((real_T)b_A[jj + jy] != 0.0) { - if (jy != 0) { - ipiv[j] = (int8_T)((j + jy) + 1); + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (int8_T)((j + iy) + 1); ix = j; - iy = j + jy; + iy += j; for (k = 0; k < 6; k++) { temp = b_A[ix]; b_A[ix] = b_A[iy]; @@ -161,42 +156,42 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) } } - loop_ub = (jp1j + mmj) + 5; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + i3 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i3; jy++) { + b_A[jy] /= b_A[c]; } } - c = 5 - j; - jy = jj + 6; - for (iy = 1; iy <= c; iy++) { - if ((real_T)b_A[jy] != 0.0) { - temp = b_A[jy] * -1.0F; - ix = jp1j; - loop_ub = (mmj + jj) + 12; - for (k = 7 + jj; k + 1 <= loop_ub; k++) { - b_A[k] += b_A[ix] * temp; + iy = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i3 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) { + b_A[ijA] += b_A[ix] * -temp; ix++; } } jy += 6; - jj += 6; + iy += 6; } } - for (jy = 0; jy < 12; jy++) { + for (i3 = 0; i3 < 12; i3++) { for (iy = 0; iy < 6; iy++) { - Y[iy + 6 * jy] = A[jy + 12 * iy]; + Y[iy + 6 * i3] = A[i3 + 12 * iy]; } } - for (iy = 0; iy < 6; iy++) { - if (ipiv[iy] != iy + 1) { + for (jy = 0; jy < 6; jy++) { + if (ipiv[jy] != jy + 1) { for (j = 0; j < 12; j++) { - temp = Y[iy + 6 * j]; - Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1]; - Y[(ipiv[iy] + 6 * j) - 1] = temp; + temp = Y[jy + 6 * j]; + Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; + Y[(ipiv[jy] + 6 * j) - 1] = temp; } } } @@ -204,10 +199,10 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) for (j = 0; j < 12; j++) { c = 6 * j; for (k = 0; k < 6; k++) { - jy = 6 * k; - if ((real_T)Y[k + c] != 0.0) { - for (iy = k + 2; iy < 7; iy++) { - Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + iy = 6 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 7; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; } } } @@ -216,19 +211,19 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) for (j = 0; j < 12; j++) { c = 6 * j; for (k = 5; k > -1; k += -1) { - jy = 6 * k; - if ((real_T)Y[k + c] != 0.0) { - Y[k + c] /= b_A[k + jy]; - for (iy = 0; iy + 1 <= k; iy++) { - Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + iy = 6 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; } } } } - for (jy = 0; jy < 6; jy++) { + for (i3 = 0; i3 < 6; i3++) { for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 6 * iy]; + y[iy + 12 * i3] = Y[i3 + 6 * iy]; } } } @@ -238,51 +233,46 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) */ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) { - int32_T jy; - int32_T iy; real32_T b_A[81]; int8_T ipiv[9]; + int32_T i2; + int32_T iy; int32_T j; - int32_T mmj; - int32_T jj; - int32_T jp1j; int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T loop_ub; + int32_T jy; + int32_T ijA; real32_T Y[108]; - for (jy = 0; jy < 9; jy++) { + for (i2 = 0; i2 < 9; i2++) { for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * jy] = B[jy + 9 * iy]; + b_A[iy + 9 * i2] = B[i2 + 9 * iy]; } - ipiv[jy] = (int8_T)(1 + jy); + ipiv[i2] = (int8_T)(1 + i2); } for (j = 0; j < 8; j++) { - mmj = -j; - jj = j * 10; - jp1j = jj + 1; - c = mmj + 9; - jy = 0; - ix = jj; - temp = (real32_T)fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { + c = j * 10; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 9 - j; k++) { ix++; - s = (real32_T)fabsf(b_A[ix]); + s = (real32_T)fabs(b_A[ix]); if (s > temp) { - jy = k - 1; + iy = k - 1; temp = s; } } - if ((real_T)b_A[jj + jy] != 0.0) { - if (jy != 0) { - ipiv[j] = (int8_T)((j + jy) + 1); + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (int8_T)((j + iy) + 1); ix = j; - iy = j + jy; + iy += j; for (k = 0; k < 9; k++) { temp = b_A[ix]; b_A[ix] = b_A[iy]; @@ -292,42 +282,42 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) } } - loop_ub = (jp1j + mmj) + 8; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + i2 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i2; jy++) { + b_A[jy] /= b_A[c]; } } - c = 8 - j; - jy = jj + 9; - for (iy = 1; iy <= c; iy++) { - if ((real_T)b_A[jy] != 0.0) { - temp = b_A[jy] * -1.0F; - ix = jp1j; - loop_ub = (mmj + jj) + 18; - for (k = 10 + jj; k + 1 <= loop_ub; k++) { - b_A[k] += b_A[ix] * temp; + iy = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i2 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) { + b_A[ijA] += b_A[ix] * -temp; ix++; } } jy += 9; - jj += 9; + iy += 9; } } - for (jy = 0; jy < 12; jy++) { + for (i2 = 0; i2 < 12; i2++) { for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * jy] = A[jy + 12 * iy]; + Y[iy + 9 * i2] = A[i2 + 12 * iy]; } } - for (iy = 0; iy < 9; iy++) { - if (ipiv[iy] != iy + 1) { + for (jy = 0; jy < 9; jy++) { + if (ipiv[jy] != jy + 1) { for (j = 0; j < 12; j++) { - temp = Y[iy + 9 * j]; - Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1]; - Y[(ipiv[iy] + 9 * j) - 1] = temp; + temp = Y[jy + 9 * j]; + Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; + Y[(ipiv[jy] + 9 * j) - 1] = temp; } } } @@ -335,10 +325,10 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) for (j = 0; j < 12; j++) { c = 9 * j; for (k = 0; k < 9; k++) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - for (iy = k + 2; iy < 10; iy++) { - Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + iy = 9 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 10; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; } } } @@ -347,19 +337,19 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) for (j = 0; j < 12; j++) { c = 9 * j; for (k = 8; k > -1; k += -1) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - Y[k + c] /= b_A[k + jy]; - for (iy = 0; iy + 1 <= k; iy++) { - Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + iy = 9 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; } } } } - for (jy = 0; jy < 9; jy++) { + for (i2 = 0; i2 < 9; i2++) { for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 9 * iy]; + y[iy + 12 * i2] = Y[i2 + 9 * iy]; } } } diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h old mode 100755 new mode 100644 index b80f34357f..d286eda5ac --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __MRDIVIDE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c old mode 100755 new mode 100644 index 341c930220..deb71dc602 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -31,32 +31,24 @@ real32_T norm(const real32_T x[3]) { real32_T y; real32_T scale; - boolean_T firstNonZero; int32_T k; real32_T absxk; real32_T t; y = 0.0F; - scale = 0.0F; - firstNonZero = TRUE; + scale = 1.17549435E-38F; for (k = 0; k < 3; k++) { - if (x[k] != 0.0F) { - absxk = (real32_T)fabsf(x[k]); - if (firstNonZero) { - scale = absxk; - y = 1.0F; - firstNonZero = FALSE; - } else if (scale < absxk) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } + absxk = (real32_T)fabs(x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; } } - return scale * (real32_T)sqrtf(y); + return scale * (real32_T)sqrt(y); } /* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h old mode 100755 new mode 100644 index 0f58dbe694..3459cbdb5f --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __NORM_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c old mode 100755 new mode 100644 index 8672c7a85c..fc602fb5ce --- a/apps/attitude_estimator_ekf/codegen/power.c +++ b/apps/attitude_estimator_ekf/codegen/power.c @@ -3,7 +3,7 @@ * * Code generation for function 'power' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -27,17 +27,17 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1); static real32_T rt_powf_snf(real32_T u0, real32_T u1) { real32_T y; - real32_T f0; real32_T f1; + real32_T f2; if (rtIsNaNF(u0) || rtIsNaNF(u1)) { y = ((real32_T)rtNaN); } else { - f0 = (real32_T)fabsf(u0); - f1 = (real32_T)fabsf(u1); + f1 = (real32_T)fabs(u0); + f2 = (real32_T)fabs(u1); if (rtIsInfF(u1)) { - if (f0 == 1.0F) { + if (f1 == 1.0F) { y = ((real32_T)rtNaN); - } else if (f0 > 1.0F) { + } else if (f1 > 1.0F) { if (u1 > 0.0F) { y = ((real32_T)rtInf); } else { @@ -48,9 +48,9 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1) } else { y = ((real32_T)rtInf); } - } else if (f1 == 0.0F) { + } else if (f2 == 0.0F) { y = 1.0F; - } else if (f1 == 1.0F) { + } else if (f2 == 1.0F) { if (u1 > 0.0F) { y = u0; } else { @@ -59,11 +59,11 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1) } else if (u1 == 2.0F) { y = u0 * u0; } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrtf(u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) { + y = (real32_T)sqrt(u0); + } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { y = ((real32_T)rtNaN); } else { - y = (real32_T)powf(u0, u1); + y = (real32_T)pow(u0, u1); } } @@ -73,11 +73,11 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1) /* * */ -void power(const real32_T a[12], real_T b, real32_T y[12]) +void power(const real32_T a[12], real32_T y[12]) { int32_T k; for (k = 0; k < 12; k++) { - y[k] = rt_powf_snf(a[k], (real32_T)b); + y[k] = rt_powf_snf(a[k], 2.0F); } } diff --git a/apps/attitude_estimator_ekf/codegen/power.h b/apps/attitude_estimator_ekf/codegen/power.h old mode 100755 new mode 100644 index a60f1bb25f..40a0d9353f --- a/apps/attitude_estimator_ekf/codegen/power.h +++ b/apps/attitude_estimator_ekf/codegen/power.h @@ -3,7 +3,7 @@ * * Code generation for function 'power' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -11,7 +11,7 @@ #define __POWER_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" @@ -29,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void power(const real32_T a[12], real_T b, real32_T y[12]); +extern void power(const real32_T a[12], real32_T y[12]); #endif /* End of code generation (power.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c new file mode 100644 index 0000000000..11e4e6f1f8 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rdivide.c @@ -0,0 +1,38 @@ +/* + * rdivide.c + * + * Code generation for function 'rdivide' + * + * C source code generated on: Tue Oct 16 15:27:58 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "rdivide.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void rdivide(const real32_T x[3], real32_T y, real32_T z[3]) +{ + int32_T i; + for (i = 0; i < 3; i++) { + z[i] = x[i] / y; + } +} + +/* End of code generation (rdivide.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h new file mode 100644 index 0000000000..4e24303748 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rdivide.h @@ -0,0 +1,34 @@ +/* + * rdivide.h + * + * Code generation for function 'rdivide' + * + * C source code generated on: Tue Oct 16 15:27:58 2012 + * + */ + +#ifndef __RDIVIDE_H__ +#define __RDIVIDE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]); +#endif +/* End of code generation (rdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c old mode 100755 new mode 100644 index 53686acc98..d20fa13fca --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h old mode 100755 new mode 100644 index 5ac1dc7943..83a3558735 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c old mode 100755 new mode 100644 index 1e1876b80f..d357102afd --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h old mode 100755 new mode 100644 index 5f1c81f676..415927709b --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h old mode 100755 new mode 100644 index 5f65f6de98..608391608f --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c old mode 100755 new mode 100644 index 2c687d7a1f..69069562b8 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h old mode 100755 new mode 100644 index d2ebd0806e..9686964ae5 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h old mode 100755 new mode 100644 index b487c8b384..77fd3ec7ae --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -26,8 +26,8 @@ * Number of bits: char: 8 short: 16 int: 32 * long: 32 native word size: 32 * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: off *=======================================================================*/ /*=======================================================================* diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c new file mode 100644 index 0000000000..88b64a06a9 --- /dev/null +++ b/apps/commander/calibration_routines.c @@ -0,0 +1,175 @@ +#include + +#include "calibration_routines.h" + + +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) { + + float x_sumplain = 0.0f; + float x_sumsq = 0.0f; + float x_sumcube = 0.0f; + + float y_sumplain = 0.0f; + float y_sumsq = 0.0f; + float y_sumcube = 0.0f; + + float z_sumplain = 0.0f; + float z_sumsq = 0.0f; + float z_sumcube = 0.0f; + + float xy_sum = 0.0f; + float xz_sum = 0.0f; + float yz_sum = 0.0f; + + float x2y_sum = 0.0f; + float x2z_sum = 0.0f; + float y2x_sum = 0.0f; + float y2z_sum = 0.0f; + float z2x_sum = 0.0f; + float z2y_sum = 0.0f; + + for (unsigned int i = 0; i < size; i++) { + + float x2 = x[i] * x[i]; + float y2 = y[i] * y[i]; + float z2 = z[i] * z[i]; + + x_sumplain += x[i]; + x_sumsq += x2; + x_sumcube += x2 * x[i]; + + y_sumplain += y[i]; + y_sumsq += y2; + y_sumcube += y2 * y[i]; + + z_sumplain += z[i]; + z_sumsq += z2; + z_sumcube += z2 * z[i]; + + xy_sum += x[i] * y[i]; + xz_sum += x[i] * z[i]; + yz_sum += y[i] * z[i]; + + x2y_sum += x2 * y[i]; + x2z_sum += x2 * z[i]; + + y2x_sum += y2 * x[i]; + y2z_sum += y2 * z[i]; + + z2x_sum += z2 * x[i]; + z2y_sum += z2 * y[i]; + } + + // + //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data + // + // P is a structure that has been computed with the data earlier. + // P.npoints is the number of elements; the length of X,Y,Z are identical. + // P's members are logically named. + // + // X[n] is the x component of point n + // Y[n] is the y component of point n + // Z[n] is the z component of point n + // + // A is the x coordiante of the sphere + // B is the y coordiante of the sphere + // C is the z coordiante of the sphere + // Rsq is the radius squared of the sphere. + // + //This method should converge; maybe 5-100 iterations or more. + // + float x_sum = x_sumplain / size; //sum( X[n] ) + float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) + float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) + float y_sum = y_sumplain / size; //sum( Y[n] ) + float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) + float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) + float z_sum = z_sumplain / size; //sum( Z[n] ) + float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) + float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) + + float XY = xy_sum / size; //sum( X[n] * Y[n] ) + float XZ = xz_sum / size; //sum( X[n] * Z[n] ) + float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) + float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) + float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) + float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) + float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) + float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) + float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) + + //Reduction of multiplications + float F0 = x_sum2 + y_sum2 + z_sum2; + float F1 = 0.5f * F0; + float F2 = -8.0f * (x_sum3 + Y2X + Z2X); + float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); + float F4 = -8.0f * (X2Z + Y2Z + z_sum3); + + //Set initial conditions: + float A = x_sum; + float B = y_sum; + float C = z_sum; + + //First iteration computation: + float A2 = A*A; + float B2 = B*B; + float C2 = C*C; + float QS = A2 + B2 + C2; + float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum); + + //Set initial conditions: + float Rsq = F0 + QB + QS; + + //First iteration computation: + float Q0 = 0.5f * (QS - Rsq); + float Q1 = F1 + Q0; + float Q2 = 8.0f * ( QS - Rsq + QB + F0 ); + float aA,aB,aC,nA,nB,nC,dA,dB,dC; + + //Iterate N times, ignore stop condition. + int n = 0; + while( n < max_iterations ){ + n++; + + //Compute denominator: + aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2); + aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2); + aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2); + aA = (aA == 0.0f) ? 1.0f : aA; + aB = (aB == 0.0f) ? 1.0f : aB; + aC = (aC == 0.0f) ? 1.0f : aC; + + //Compute next iteration + nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA); + nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB); + nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC); + + //Check for stop condition + dA = (nA - A); + dB = (nB - B); + dC = (nC - C); + if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; } + + //Compute next iteration's values + A = nA; + B = nB; + C = nC; + A2 = A*A; + B2 = B*B; + C2 = C*C; + QS = A2 + B2 + C2; + QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum); + Rsq = F0 + QB + QS; + Q0 = 0.5f * (QS - Rsq); + Q1 = F1 + Q0; + Q2 = 8.0f * ( QS - Rsq + QB + F0 ); + } + + *sphere_x = A; + *sphere_y = B; + *sphere_z = C; + *sphere_radius = sqrtf(Rsq); + + return 0; +} diff --git a/apps/commander/calibration_routines.h b/apps/commander/calibration_routines.h new file mode 100644 index 0000000000..c5a184341e --- /dev/null +++ b/apps/commander/calibration_routines.h @@ -0,0 +1,21 @@ + +/** + * Least-squares fit of a sphere to a set of points. + * + * Fits a sphere to a set of points on the sphere surface. + * + * @param x point coordinates on the X axis + * @param y point coordinates on the Y axis + * @param z point coordinates on the Z axis + * @param size number of points + * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. + * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times. + * @param sphere_x coordinate of the sphere center on the X axis + * @param sphere_y coordinate of the sphere center on the Y axis + * @param sphere_z coordinate of the sphere center on the Z axis + * @param sphere_radius sphere radius + * + * @return 0 on success, 1 on failure + */ +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c569a6aa4a..a3dccfd730 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -57,8 +57,8 @@ #include #include #include -#include #include +#include #include "state_machine_helper.h" #include "systemlib/systemlib.h" #include @@ -83,6 +83,8 @@ #include #include +#include "calibration_routines.h" + PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ @@ -114,8 +116,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 +126,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[]); @@ -136,6 +137,7 @@ int commander_thread_main(int argc, char *argv[]); static int buzzer_init(void); static void buzzer_deinit(void); +static void tune_confirm(); static int led_init(void); static void led_deinit(void); static int led_toggle(int led); @@ -164,7 +166,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() { @@ -256,6 +258,10 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } +void tune_confirm() { + ioctl(buzzer, TONE_SET_ALARM, 3); +} + static const char *parameter_file = "/eeprom/parameters"; static int pm_save_eeprom(bool only_unsaved) @@ -289,17 +295,31 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_mag_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; - /* 30 seconds */ - int calibration_interval_ms = 30 * 1000; + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 2000; unsigned int calibration_counter = 0; - float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - int fd = open(MAG_DEVICE_PATH, 0); + // XXX old cal + // * FLT_MIN is not the most negative float number, + // * but the smallest number by magnitude float can + // * represent. Use -FLT_MAX to initialize the most + // * negative number + + // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -308,42 +328,93 @@ 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"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + 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) { + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'Z', 'X', 'Y'}; + int axis_index = -1; + + float *x = malloc(sizeof(float) * calibration_maxcount); + float *y = malloc(sizeof(float) * calibration_maxcount); + float *z = malloc(sizeof(float) * calibration_maxcount); + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_confirm(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // 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); + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + /* get min/max values */ - if (raw.magnetometer_ga[0] < mag_min[0]) { - mag_min[0] = raw.magnetometer_ga[0]; - } - else if (raw.magnetometer_ga[0] > mag_max[0]) { - mag_max[0] = raw.magnetometer_ga[0]; - } + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } - 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,68 +424,89 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } } - mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + fprintf(stderr, "[commander] Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + fprintf(stderr, "[commander] Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + fprintf(stderr, "[commander] Setting Z mag scale 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"); + } + + printf("[mag cal]\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + 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)"); + } /* disable calibration mode */ status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - float mag_offset[3]; - - /** - * The offset is subtracted from the sensor values, so the result is the - * POSITIVE number that has to be subtracted from the sensor data - * to shift the center to zero - * - * offset = max - ((max - min) / 2.0f) - * - * which reduces to - * - * offset = (max + min) / 2.0f - */ - - mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; - mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; - mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - - printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)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"); - } - - 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_ZOFF"), &(mag_offset[2]))) { - fprintf(stderr, "[commander] Setting Z 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); - - /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); - if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); - } - - mavlink_log_info(mavlink_fd, "[commander] magnetometer calibration finished"); - - close(sub_sensor_combined); + close(sub_mag); } void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) @@ -467,45 +559,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!"); - } - - /* 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"); - } - - /* exit to gyro calibration mode */ + /* exit gyro calibration mode */ status->flag_preflight_gyro_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished"); + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - 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]); + 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!"); + } + + /* 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"); + } + + // 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); } @@ -519,7 +617,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); - const int calibration_count = 5000; + const int calibration_count = 2500; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; @@ -560,61 +658,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_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!"); + } + + 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"); + } + + //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)"); } - 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_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); - - /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); - if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); - } - /* 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); } @@ -625,6 +733,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* result of the command */ uint8_t result = MAV_RESULT_UNSUPPORTED; + /* announce command handling */ + ioctl(buzzer, TONE_SET_ALARM, 1); + /* supported command handling start */ @@ -732,8 +843,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"); + tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); + tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -750,8 +863,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"); + tune_confirm(); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); + tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -768,7 +883,9 @@ 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 accel calibration"); + tune_confirm(); do_accel_calibration(status_pub, ¤t_status); + tune_confirm(); mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; @@ -798,6 +915,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta default: { mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); result = MAV_RESULT_UNSUPPORTED; + usleep(200000); + /* announce command rejection */ + ioctl(buzzer, TONE_SET_ALARM, 4); } break; } @@ -813,37 +933,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 */ @@ -954,7 +1043,7 @@ int commander_main(int argc, char *argv[]) deamon_task = task_spawn("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 50, - 4096, + 8000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; @@ -992,7 +1081,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 */ @@ -1034,11 +1123,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); @@ -1083,6 +1167,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; @@ -1112,6 +1201,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; @@ -1180,7 +1278,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { /* For less than 20%, start be slightly annoying at 1 Hz */ ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 2); + tune_confirm(); } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -1470,7 +1568,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 */ @@ -1480,6 +1578,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/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 7d766bcdb2..b21f3858ff 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -504,7 +504,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flag_control_manual_enabled = true; /* enable attitude control per default */ current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = false; // XXX + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -519,7 +519,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; current_status->flag_control_manual_enabled = true; current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = false; // XXX + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -534,7 +534,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; current_status->flag_control_manual_enabled = true; current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = false; // XXX + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 1f5bb998e7..7ba9dd6956 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -99,7 +99,13 @@ 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) + +/** excite strap */ +#define MAGIOCEXSTRAP _MAGIOC(6) #endif /* _DRV_MAG_H */ diff --git a/nuttx/configs/px4fmu/include/drv_tone_alarm.h b/apps/drivers/drv_tone_alarm.h similarity index 98% rename from nuttx/configs/px4fmu/include/drv_tone_alarm.h rename to apps/drivers/drv_tone_alarm.h index b24c85c8d0..27b146de9b 100644 --- a/nuttx/configs/px4fmu/include/drv_tone_alarm.h +++ b/apps/drivers/drv_tone_alarm.h @@ -63,8 +63,6 @@ #define _TONE_ALARM_BASE 0x7400 #define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) -extern int tone_alarm_init(void); - /* structure describing one note in a tone pattern */ struct tone_note { uint8_t pitch; @@ -127,4 +125,4 @@ enum tone_pitch { TONE_NOTE_MAX }; -#endif /* DRV_TONE_ALARM_H_ */ \ No newline at end of file +#endif /* DRV_TONE_ALARM_H_ */ diff --git a/apps/drivers/hmc5883/Makefile b/apps/drivers/hmc5883/Makefile index aa4a397fb3..4d7cb4e7b9 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 c0a5f4049a..8e78825c34 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -175,6 +175,36 @@ 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(struct file *filp, unsigned enable); + + /** + * 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 positive strap, -1 to enable + * negative strap, 0 to set to normal mode + */ + int set_excitement(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. @@ -254,9 +284,9 @@ HMC5883::HMC5883(int bus) : _next_report(0), _oldest_report(0), _reports(nullptr), + _range_scale(0), /* default range scale from counts to gauss */ + _range_ga(1.3f), _mag_topic(-1), - _range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */ - _range_ga(0.88f), _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 +338,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 +585,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 +603,10 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 0; case MAGIOCCALIBRATE: - /* XXX perform auto-calibration */ - return -EINVAL; + return calibrate(filp, arg); + + case MAGIOCEXSTRAP: + return set_excitement(arg); default: /* give it to the superclass */ @@ -718,6 +813,194 @@ out: return ret; } +int HMC5883::calibrate(struct file *filp, unsigned enable) +{ + struct mag_report report; + ssize_t sz; + int ret = 1; + + // XXX do something smarter here + int fd = (int)enable; + + struct mag_scale mscale_previous = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + float avg_excited[3] = {0.0f, 0.0f, 0.0f}; + unsigned i; + + warnx("starting mag scale calibration"); + + /* do a simple demand read */ + sz = read(filp, (char*)&report, sizeof(report)); + if (sz != sizeof(report)) { + warn("immediate read failed"); + ret = 1; + goto out; + } + + warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + warnx("sampling 500 samples for scaling offset"); + + /* set the queue depth to 10 */ + if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { + warn("failed to set queue depth"); + ret = 1; + goto out; + } + + /* start the sensor polling at 50 Hz */ + if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { + warn("failed to set 2Hz poll rate"); + ret = 1; + goto out; + } + + /* Set to 2.5 Gauss */ + if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + warnx("failed to set 2.5 Ga range"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { + warnx("failed to enable sensor calibration mode"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to get scale / offsets for mag"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set null scale / offsets for mag"); + ret = 1; + goto out; + } + + /* read the sensor 10x and report each value */ + for (i = 0; i < 500; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + goto out; + } 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); + } + + avg_excited[0] /= i; + avg_excited[1] /= i; + avg_excited[2] /= i; + + warnx("done. Performed %u reads", i); + warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); + + float scaling[3]; + + /* calculate axis scaling */ + scaling[0] = fabsf(1.16f / avg_excited[0]); + /* second axis inverted */ + scaling[1] = fabsf(1.16f / -avg_excited[1]); + scaling[2] = fabsf(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"); + goto out; + } + + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { + warnx("failed to disable sensor calibration mode"); + goto out; + } + + /* 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(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to set new scale / offsets for mag"); + goto out; + } + + ret = OK; + + out: + if (ret == OK) { + warnx("mag scale calibration successfully finished."); + } else { + warnx("mag scale calibration failed."); + } + return ret; +} + +int HMC5883::set_excitement(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 (((int)enable) < 0) { + conf_reg |= 0x01; + } else if (enable > 0) { + conf_reg |= 0x02; + } 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 +1058,7 @@ void start(); void test(); void reset(); void info(); +int calibrate(); /** * Start the driver. @@ -872,6 +1156,69 @@ 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() +{ + 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); + + if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { + warnx("failed to enable sensor calibration mode"); + } + + close(fd); + + if (ret == OK) { + errx(0, "PASS"); + } else { + errx(1, "FAIL"); + } +} + /** * Reset the driver. */ @@ -930,8 +1277,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 9401fdd4a5..e9b60b01c9 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/drivers/stm32/tone_alarm/Makefile b/apps/drivers/stm32/tone_alarm/Makefile new file mode 100644 index 0000000000..d2ddf95340 --- /dev/null +++ b/apps/drivers/stm32/tone_alarm/Makefile @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Tone alarm driver +# + +APPNAME = tone_alarm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/nuttx/configs/px4fmu/src/drv_tone_alarm.c b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp similarity index 61% rename from nuttx/configs/px4fmu/src/drv_tone_alarm.c rename to apps/drivers/stm32/tone_alarm/tone_alarm.cpp index 958a189047..bfde83cde9 100644 --- a/nuttx/configs/px4fmu/src/drv_tone_alarm.c +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -31,46 +31,61 @@ * ****************************************************************************/ -/* - * Speaker driver supporting alarm sequences. +/** + * Driver for the PX4 audio alarm port, /dev/tone_alarm. * - * Works with any of the 'generic' STM32 timers that has an output - * pin, does not require an interrupt. + * The tone_alarm driver supports a set of predefined "alarm" + * patterns and one user-supplied pattern. Patterns are ordered by + * priority, with a higher-priority pattern interrupting any + * lower-priority pattern that might be playing. * - * Depends on the HRT timer. + * The TONE_SET_ALARM ioctl can be used to select a predefined + * alarm pattern, from 1 - . Selecting pattern zero silences + * the alarm. + * + * To supply a custom pattern, write an array of 1 - tone_note + * structures to /dev/tone_alarm. The custom pattern has a priority + * of zero. + * + * Patterns will normally play once and then silence (if a pattern + * was overridden it will not resume). A pattern may be made to + * repeat by inserting a note with the duration set to + * DURATION_REPEAT. This pattern will loop until either a + * higher-priority pattern is started or pattern zero is requested + * via the ioctl. */ #include -#include -#include + +#include +#include #include +#include #include - -#include -#include -#include -#include -#include +#include #include +#include +#include #include +#include #include -#include #include -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" +#include +#include +#include -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" +#include +#include +#include -#ifdef CONFIG_TONE_ALARM -# ifndef CONFIG_HRT_TIMER -# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER -# endif +#include + +#ifndef CONFIG_HRT_TIMER +# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER +#endif /* Tone alarm configuration */ #if TONE_ALARM_TIMER == 2 @@ -123,7 +138,7 @@ # error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11 # endif #else -# error Must set TONE_ALARM_TIMER to a generic timer if CONFIG_TONE_ALARM is set +# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver. #endif #if TONE_ALARM_CHANNEL == 1 @@ -147,9 +162,10 @@ # define TONE_CCER (1 << 12) # define TONE_rCCR rCCR4 #else -# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 +# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver. #endif + /* * Timer register accessors */ @@ -174,11 +190,42 @@ #define rDCR REG(STM32_GTIM_DCR_OFFSET) #define rDMAR REG(STM32_GTIM_DMAR_OFFSET) -#define TONE_MAX_PATTERN 6 -#define TONE_MAX_PATTERN_LEN 40 +class ToneAlarm : public device::CDev +{ +public: + ToneAlarm(); + ~ToneAlarm(); -/* predefined patterns for alarms 1-TONE_MAX_PATTERN */ -const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = { + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + +private: + static const unsigned _max_pattern = 6; + static const unsigned _pattern_none = _max_pattern + 1; + static const unsigned _pattern_user = 0; + static const unsigned _max_pattern_len = 40; + static const unsigned _note_max = TONE_NOTE_MAX; + + static const tone_note _patterns[_max_pattern][_max_pattern_len]; + static const uint16_t _notes[_note_max]; + + unsigned _current_pattern; + unsigned _next_note; + + hrt_call _note_end; + tone_note _user_pattern[_max_pattern_len]; + + static void next_trampoline(void *arg); + void next(); + bool check(tone_note *pattern); + void stop(); +}; + + +/* predefined patterns for alarms 1-_max_pattern */ +const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = { { {TONE_NOTE_A7, 12}, {TONE_NOTE_D8, 12}, @@ -193,7 +240,7 @@ const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = { {TONE_NOTE_D8, 4}, {TONE_NOTE_C8, 4}, }, - {{TONE_NOTE_B6, 100}}, + {{TONE_NOTE_B6, 100}, {TONE_NOTE_B6, DURATION_REPEAT}}, {{TONE_NOTE_C7, 100}}, {{TONE_NOTE_D7, 100}}, {{TONE_NOTE_E7, 100}}, @@ -217,7 +264,6 @@ const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = { {TONE_NOTE_G5S, 40}, {TONE_NOTE_F5, 40}, {TONE_NOTE_F5, 60}, - {TONE_NOTE_A5S, 40}, {TONE_NOTE_C6S, 20}, {TONE_NOTE_F6, 40}, @@ -239,7 +285,7 @@ const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = { } }; -static uint16_t notes[TONE_NOTE_MAX] = { +const uint16_t ToneAlarm::_notes[_note_max] = { 63707, /* E4 */ 60132, /* F4 */ 56758, /* F#4/Gb4 */ @@ -290,46 +336,46 @@ static uint16_t notes[TONE_NOTE_MAX] = { 4218 /* D#8/Eb8 */ }; -/* current state of the tone driver */ -struct state { - int pattern; /* current pattern */ -#define PATTERN_NONE -1 -#define PATTERN_USER 0 - int note; /* next note to play */ - struct hrt_call note_end; - struct tone_note user_pattern[TONE_MAX_PATTERN_LEN]; /* user-supplied pattern (plays at pattern 0) */ -}; +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); -static struct state tone_state; -static int tone_write(struct file *filp, const char *buffer, size_t len); -static int tone_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations tone_fops = { - .write = tone_write, - .ioctl = tone_ioctl -}; - -static void tone_next(void); -static bool tone_ok(struct tone_note *pattern); -int -tone_alarm_init(void) +ToneAlarm::ToneAlarm() : + CDev("tone_alarm", "/dev/tone_alarm"), + _current_pattern(_pattern_none), + _next_note(0) { - /* configure the GPIO */ + // enable debug() calls + //_debug_enabled = true; +} + +ToneAlarm::~ToneAlarm() +{ +} + +int +ToneAlarm::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) + return ret; + + /* configure the GPIO to the idle state */ stm32_configgpio(GPIO_TONE_ALARM); /* clock/power on our timer */ modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); - /* default the state */ - tone_state.pattern = -1; - /* initialise the timer */ rCR1 = 0; rCR2 = 0; rSMCR = 0; rDIER = 0; - rCCER = 0; /* unlock CCMR* registers */ + rCCER &= TONE_CCER; /* unlock CCMR* registers */ rCCMR1 = TONE_CCMR1; rCCMR2 = TONE_CCMR2; rCCER = TONE_CCER; @@ -346,153 +392,163 @@ tone_alarm_init(void) */ rPSC = 1; - tone_state.pattern = PATTERN_NONE; - tone_state.note = 0; + /* make sure the timer is running */ + rCR1 = GTIM_CR1_CEN; - /* play the startup tune */ - tone_ioctl(NULL, TONE_SET_ALARM, 1); - - /* register the device */ - return register_driver("/dev/tone_alarm", &tone_fops, 0666, NULL); + debug("ready"); + return OK; } -static int -tone_ioctl(struct file *filep, int cmd, unsigned long arg) +int +ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) { int result = 0; - int new = (int)arg; + unsigned new_pattern = arg; - irqstate_t flags = irqsave(); + debug("ioctl %i %u", cmd, new_pattern); + +// irqstate_t flags = irqsave(); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - if (new == 0) { + debug("TONE_SET_ALARM %u", arg); + if (new_pattern == 0) { /* cancel any current alarm */ - tone_state.pattern = PATTERN_NONE; - tone_next(); + _current_pattern = _pattern_none; + next(); - } else if (new > TONE_MAX_PATTERN) { + } else if (new_pattern > _max_pattern) { /* not a legal alarm value */ result = -ERANGE; - } else if (new > tone_state.pattern) { + } else if ((_current_pattern == _pattern_none) || (new_pattern > _current_pattern)) { /* higher priority than the current alarm */ - tone_state.pattern = new; - tone_state.note = 0; + _current_pattern = new_pattern; + _next_note = 0; /* and start playing it */ - tone_next(); + next(); + } else { + /* current pattern is higher priority than the new pattern, ignore */ } break; default: - result = -EINVAL; + result = -ENOTTY; break; } - irqrestore(flags); +// irqrestore(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) + result = CDev::ioctl(filp, cmd, arg); + return result; } -static int -tone_write(struct file *filp, const char *buffer, size_t len) +int +ToneAlarm::write(file *filp, const char *buffer, size_t len) { irqstate_t flags; /* sanity-check the size of the write */ - if (len > (TONE_MAX_PATTERN_LEN * sizeof(struct tone_note))) + if (len > (_max_pattern_len * sizeof(tone_note))) return -EFBIG; - if ((len % sizeof(struct tone_note)) || (len == 0)) + if ((len % sizeof(tone_note)) || (len == 0)) return -EIO; - if (!tone_ok((struct tone_note *)buffer)) + if (!check((tone_note *)buffer)) return -EIO; flags = irqsave(); /* if we aren't playing an alarm tone */ - if (tone_state.pattern <= PATTERN_USER) { + if (_current_pattern <= _pattern_user) { /* reset the tone state to play the new user pattern */ - tone_state.pattern = PATTERN_USER; - tone_state.note = 0; + _current_pattern = _pattern_user; + _next_note = 0; /* copy in the new pattern */ - memset(tone_state.user_pattern, 0, sizeof(tone_state.user_pattern)); - memcpy(tone_state.user_pattern, buffer, len); + memset(_user_pattern, 0, sizeof(_user_pattern)); + memcpy(_user_pattern, buffer, len); /* and start it */ - tone_next(); + debug("starting user pattern"); + next(); } irqrestore(flags); return len; } -static void -tone_next(void) +void +ToneAlarm::next_trampoline(void *arg) { - const struct tone_note *np; + ToneAlarm *ta = (ToneAlarm *)arg; + + ta->next(); +} + +void +ToneAlarm::next(void) +{ + const tone_note *np; /* stop the current note */ - rCR1 = 0; + rCCER &= ~TONE_CCER; /* if we are no longer playing a pattern, we have nothing else to do here */ - if (tone_state.pattern == PATTERN_NONE) { + if (_current_pattern == _pattern_none) { + stop(); return; } - /* if the current pattern has ended, clear the pattern and stop */ - if (tone_state.note == TONE_NOTE_MAX) { - tone_state.pattern = PATTERN_NONE; - return; - } + ASSERT(_next_note < _note_max); /* find the note to play */ - if (tone_state.pattern == PATTERN_USER) { - np = &tone_state.user_pattern[tone_state.note]; + if (_current_pattern == _pattern_user) { + np = &_user_pattern[_next_note]; } else { - np = &patterns[tone_state.pattern - 1][tone_state.note]; + np = &_patterns[_current_pattern - 1][_next_note]; } /* work out which note is next */ - tone_state.note++; - if (tone_state.note >= TONE_NOTE_MAX) { + _next_note++; + if (_next_note >= _note_max) { /* hit the end of the pattern, stop */ - tone_state.pattern = PATTERN_NONE; + _current_pattern = _pattern_none; } else if (np[1].duration == DURATION_END) { /* hit the end of the pattern, stop */ - tone_state.pattern = PATTERN_NONE; + _current_pattern = _pattern_none; } else if (np[1].duration == DURATION_REPEAT) { /* next note is a repeat, rewind in preparation */ - tone_state.note = 0; + _next_note = 0; } /* set the timer to play the note, if required */ if (np->pitch <= TONE_NOTE_SILENCE) { /* set reload based on the pitch */ - rARR = notes[np->pitch]; + rARR = _notes[np->pitch]; /* force an update, reloads the counter and all registers */ rEGR = GTIM_EGR_UG; - /* start the timer */ - rCR1 = GTIM_CR1_CEN; + /* enable the output */ + rCCER |= TONE_CCER; } /* arrange a callback when the note/rest is done */ - hrt_call_after(&tone_state.note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)tone_next, NULL); - + hrt_call_after(&_note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)next_trampoline, this); } -static bool -tone_ok(struct tone_note *pattern) +bool +ToneAlarm::check(tone_note *pattern) { - int i; - - for (i = 0; i < TONE_NOTE_MAX; i++) { + for (unsigned i = 0; i < _note_max; i++) { /* first note must not be repeat or end */ if ((i == 0) && @@ -502,10 +558,75 @@ tone_ok(struct tone_note *pattern) break; /* pitch must be legal */ - if (pattern[i].pitch >= TONE_NOTE_MAX) + if (pattern[i].pitch >= _note_max) return false; } return true; } -#endif /* CONFIG_TONE_ALARM */ \ No newline at end of file +void +ToneAlarm::stop() +{ + /* stop the current note */ + rCCER &= ~TONE_CCER; + + /* + * Make sure the GPIO is not driving the speaker. + * + * XXX this presumes PX4FMU and the onboard speaker driver FET. + */ + stm32_gpiowrite(GPIO_TONE_ALARM, 0); +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +ToneAlarm *g_dev; + +int +play_pattern(unsigned pattern) +{ + int fd, ret; + + fd = open("/dev/tone_alarm", 0); + if (fd < 0) + err(1, "/dev/tone_alarm"); + + warnx("playing pattern %u", pattern); + ret = ioctl(fd, TONE_SET_ALARM, pattern); + if (ret != 0) + err(1, "TONE_SET_ALARM"); + exit(0); +} + +} // namespace + +int +tone_alarm_main(int argc, char *argv[]) +{ + unsigned pattern; + + /* start the driver lazily */ + if (g_dev == nullptr) { + g_dev = new ToneAlarm; + + if (g_dev == nullptr) + errx(1, "couldn't allocate the ToneAlarm driver"); + if (g_dev->init() != OK) { + delete g_dev; + errx(1, "ToneAlarm init failed"); + } + } + + if ((argc > 1) && !strcmp(argv[1], "start")) + play_pattern(1); + if ((argc > 1) && !strcmp(argv[1], "stop")) + play_pattern(0); + if ((pattern = strtol(argv[1], nullptr, 10)) != 0) + play_pattern(pattern); + + errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); +} diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index ad08247e10..8629c2f110 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: @author Ivan Ovinnikov + * Modifications: Doug Weibel * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ -// Workflow test comment - DEW + /** * @file fixedwing_control.c * Implementation of a fixed wing attitude and position controller. @@ -86,27 +87,61 @@ static void usage(const char *reason); * Controller parameters, accessible via MAVLink * */ -PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f); -PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f); +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians struct fw_att_control_params { - float roll_pos_p; - float roll_pos_i; - float roll_pos_awu; - float roll_pos_lim; + float rollrate_p; + float rollrate_i; + float rollrate_awu; + float rollrate_lim; + float roll_p; + float roll_lim; + float pitchrate_p; + float pitchrate_i; + float pitchrate_awu; + float pitchrate_lim; + float pitch_p; + float pitch_lim; }; struct fw_att_control_param_handles { - param_t roll_pos_p; - param_t roll_pos_i; - param_t roll_pos_awu; - param_t roll_pos_lim; + param_t rollrate_p; + param_t rollrate_i; + param_t rollrate_awu; + param_t rollrate_lim; + param_t roll_p; + param_t roll_lim; + param_t pitchrate_p; + param_t pitchrate_i; + param_t pitchrate_awu; + param_t pitchrate_lim; + param_t pitch_p; + param_t pitch_lim; }; -// XXX outsource position control to a separate app some time +// TO_DO - Navigation control will be moved to a separate app +// Attitude control will just handle the inner angle and rate loops +// to control pitch and roll, and turn coordination via rudder and +// possibly throttle compensation for battery voltage sag. PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); @@ -147,9 +182,9 @@ static int pos_parameters_update(const struct fw_pos_control_param_handles *h, s /** - * The fixed wing control main loop. + * The fixed wing control main thread. * - * This loop executes continously and calculates the control + * The main loop executes continously and calculates the control * response. * * @param argc number of arguments @@ -223,19 +258,31 @@ int fixedwing_control_thread_main(int argc, char *argv[]) pos_parameters_init(&hpos); pos_parameters_update(&hpos, &ppos); - PID_t roll_pos_controller; - pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, - PID_MODE_DERIVATIV_SET); +// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere + PID_t roll_rate_controller; + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu, + p.rollrate_lim,PID_MODE_DERIVATIV_NONE); + PID_t roll_angle_controller; + pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f, + p.roll_lim,PID_MODE_DERIVATIV_NONE); + + PID_t pitch_rate_controller; + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu, + p.pitchrate_lim,PID_MODE_DERIVATIV_NONE); + PID_t pitch_angle_controller; + pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f, + p.pitch_lim,PID_MODE_DERIVATIV_NONE); PID_t heading_controller; pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - PID_MODE_DERIVATIV_SET); + 100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit // XXX remove in production /* advertise debug value */ struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + // This is the top of the main loop while(!thread_should_exit) { struct pollfd fds[1] = { @@ -251,11 +298,20 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } else { // FIXME SUBSCRIBE - if (loopcounter % 20 == 0) { + if (loopcounter % 100 == 0) { att_parameters_update(&h, &p); pos_parameters_update(&hpos, &ppos); - pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu); - pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, + p.rollrate_awu, p.rollrate_lim); + pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, + 0.0f, p.roll_lim); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, + p.pitchrate_awu, p.pitchrate_lim); + pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, + 0.0f, p.pitch_lim); + pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); +//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n", +//p.rollrate_p, p.rollrate_i, p.rollrate_lim); } /* if position updated, run position control loop */ @@ -359,14 +415,25 @@ int fixedwing_control_thread_main(int argc, char *argv[]) printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); } - // actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, - // att.roll, att.rollspeed, deltaTpos); - actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, - att.roll, att.rollspeed, deltaTpos); - actuators.control[1] = 0; - actuators.control[2] = 0; + // actuator control[0] is aileron (or elevon roll control) + // Commanded roll rate from P controller on roll angle + float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body, + att.roll, 0.0f, deltaTpos); + // actuator control from PI controller on roll rate + actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command, + att.rollspeed, 0.0f, deltaTpos); + + // actuator control[1] is elevator (or elevon pitch control) + // Commanded pitch rate from P controller on pitch angle + float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body, + att.pitch, 0.0f, deltaTpos); + // actuator control from PI controller on pitch rate + actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command, + att.pitchspeed, 0.0f, deltaTpos); + + // actuator control[3] is throttle actuators.control[3] = att_sp.thrust; - + /* publish attitude setpoint (for MAVLink) */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -446,21 +513,38 @@ int fixedwing_control_main(int argc, char *argv[]) static int att_parameters_init(struct fw_att_control_param_handles *h) { /* PID parameters */ - h->roll_pos_p = param_find("FW_ROLL_POS_P"); - h->roll_pos_i = param_find("FW_ROLL_POS_I"); - h->roll_pos_lim = param_find("FW_ROLL_POS_LIM"); - h->roll_pos_awu = param_find("FW_ROLL_POS_AWU"); - + + h->rollrate_p = param_find("FW_ROLLRATE_P"); + h->rollrate_i = param_find("FW_ROLLRATE_I"); + h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); + h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); + h->roll_p = param_find("FW_ROLL_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitchrate_p = param_find("FW_PITCHRATE_P"); + h->pitchrate_i = param_find("FW_PITCHRATE_I"); + h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); + h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); + h->pitch_p = param_find("FW_PITCH_P"); + h->pitch_lim = param_find("FW_PITCH_LIM"); + return OK; } static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) { - param_get(h->roll_pos_p, &(p->roll_pos_p)); - param_get(h->roll_pos_i, &(p->roll_pos_i)); - param_get(h->roll_pos_lim, &(p->roll_pos_lim)); - param_get(h->roll_pos_awu, &(p->roll_pos_awu)); - + param_get(h->rollrate_p, &(p->rollrate_p)); + param_get(h->rollrate_i, &(p->rollrate_i)); + param_get(h->rollrate_awu, &(p->rollrate_awu)); + param_get(h->rollrate_lim, &(p->rollrate_lim)); + param_get(h->roll_p, &(p->roll_p)); + param_get(h->roll_lim, &(p->roll_lim)); + param_get(h->pitchrate_p, &(p->pitchrate_p)); + param_get(h->pitchrate_i, &(p->pitchrate_i)); + param_get(h->pitchrate_awu, &(p->pitchrate_awu)); + param_get(h->pitchrate_lim, &(p->pitchrate_lim)); + param_get(h->pitch_p, &(p->pitch_p)); + param_get(h->pitch_lim, &(p->pitch_lim)); + return OK; } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 0592d03775..698e43f96f 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * 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.c * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier */ #include @@ -58,30 +60,19 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include +#include #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 +80,46 @@ 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_thread_main(int argc, char *argv[]); + +/* thread state */ +volatile bool thread_should_exit = false; +static volatile bool thread_running = false; static int mavlink_task; -/* terminate MAVLink on user request - disabled by default */ -static bool mavlink_link_termination_allowed = false; - -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; - /* pthreads */ static pthread_t receive_thread; static pthread_t uorb_receive_thread; +/* terminate MAVLink on user request - disabled by default */ +static bool mavlink_link_termination_allowed = false; + +mavlink_system_t mavlink_system = { + 100, + 50, + MAV_TYPE_QUADROTOR, + 0, + 0, + 0 +}; // System ID, 1-255, Component/Subsystem ID, 1-255 + +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; + +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); + /* Allocate storage space for waypoints */ -mavlink_wpm_storage wpm_s; +static mavlink_wpm_storage wpm_s; +mavlink_wpm_storage *wpm = &wpm_s; -/** Global position */ -static struct vehicle_global_position_s global_pos; +bool mavlink_hil_enabled = false; -/** 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; - -static struct offboard_control_setpoint_s offboard_control_sp; - -static struct vehicle_command_s vcmd; - -static struct actuator_armed_s armed; - -static struct vehicle_vicon_position_s vicon_position; - -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; - -static char mavlink_message_string[51] = {0}; - -static int baudrate = 57600; +/* protocol interface */ +static int uart; +static int baudrate; +bool gcs_link = true; /* interface mode */ static enum { @@ -154,207 +127,16 @@ 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_logbuffer lb; -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); - -/** - * 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); -} +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +static void usage(void); -int set_hil_on_off(bool hil_enabled) +int +set_hil_on_off(bool hil_enabled) { int ret = OK; @@ -363,9 +145,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 +180,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 +188,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 +267,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 +274,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 +313,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 @@ -1137,7 +334,9 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; - strncpy(mavlink_message_string, txt, 51); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); total_counter++; return OK; } @@ -1152,349 +351,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 +449,30 @@ 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)); +} + +/* + * Internal function to give access to the channel status for each channel + */ +mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_status[chan]; +} + +/* + * Internal function to give access to the channel buffer for each channel + */ +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) +{ + static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_buffer[chan]; +} + void mavlink_update_system(void) { static bool initialized = false; @@ -1631,113 +511,73 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { - wpm = &wpm_s; + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&lb, 5); - /* 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)); - - /* print welcome text */ - printf("[mavlink] MAVLink v1.0 serial interface starting..\n"); - - /* reate the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); - - /* default values for arguments */ - char *uart_name = "/dev/ttyS1"; + int ch; + char *device_name = "/dev/ttyS1"; baudrate = 57600; - /* read program arguments */ - int i; + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; - for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */ + 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; - 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 'd': + device_name = optarg; + break; + + 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); + /* print welcome text */ + warnx("MAVLink v1.0 serial interface starting..."); - if (uart < 0) { - printf("[mavlink] FAILED to open %s, terminating.\n", uart_name); - goto exit_cleanup; - } - - /* Flush UART */ + /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); + /* 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); + + /* 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(); - /* 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 */ + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); - - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - pthread_attr_setstacksize(&receiveloop_attr, 2048); - pthread_create(&receive_thread, &receiveloop_attr, receiveloop, &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); @@ -1761,14 +601,14 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 100); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else { @@ -1787,23 +627,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 +647,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 +676,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 +687,13 @@ 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_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } /* sleep 15 ms */ usleep(15000); @@ -1854,54 +704,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 ] [-b ] [-e/--exit-allowed]\n\n"); + fprintf(stderr, "usage: mavlink start [-d ] [-b ] [-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 +739,24 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (const char**)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 26bc26fdfc..de8bc4ae78 100644 --- a/apps/mavlink/mavlink_bridge_header.h +++ b/apps/mavlink/mavlink_bridge_header.h @@ -43,9 +43,12 @@ #define MAVLINK_USE_CONVENIENCE_FUNCTIONS -//use efficient approach, see mavlink_helpers.h +/* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes +#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer +#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status + #include "v1.0/mavlink_types.h" #include @@ -62,28 +65,15 @@ */ 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; +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); - 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); - } - } -} +mavlink_status_t* mavlink_get_channel_status(uint8_t chan); +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h new file mode 100644 index 0000000000..95790db934 --- /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 + * + * 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_log.c b/apps/mavlink/mavlink_log.c new file mode 100644 index 0000000000..660e282f86 --- /dev/null +++ b/apps/mavlink/mavlink_log.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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_log.c + * MAVLink text logging. + * + * @author Lorenz Meier + */ + +#include + +#include "mavlink_log.h" + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); +} + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { + return lb->count == lb->size; +} + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { + return lb->count == 0; +} + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + } else { + ++lb->count; + } +} + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { + if (!mavlink_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + return 0; + } else { + return 1; + } +} \ No newline at end of file diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 1ad1364be9..cb6fd9d98c 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * 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_log.h * MAVLink text logging. + * + * @author Lorenz Meier */ #ifndef MAVLINK_LOG @@ -79,5 +81,28 @@ */ #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +struct mavlink_logmessage { + char text[51]; + unsigned char severity; +}; + +struct mavlink_logbuffer { + unsigned int start; + // unsigned int end; + unsigned int size; + int count; + struct mavlink_logmessage *elems; +}; + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); + #endif diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 2e15174bb0..8874fe5287 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 + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * 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 */ #include "mavlink_parameters.h" diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h index 950f82d2de..37fd442869 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 + * Author: Lorenz Meier * * 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 */ /* 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 0000000000..5507467949 --- /dev/null +++ b/apps/mavlink/mavlink_receiver.c @@ -0,0 +1,497 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#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; + +extern bool gcs_link; + +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) { + + /* switch to a receiving link mode */ + gcs_link = false; + + /* + * rate control mode - defined by MAVLink + */ + + uint8_t ml_mode = 0; + bool ml_armed = false; + + 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; + + 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); + + struct sched_param param; + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + 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 0000000000..d2be9a88d6 --- /dev/null +++ b/apps/mavlink/missionlib.c @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#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 0000000000..3439c185dd --- /dev/null +++ b/apps/mavlink/missionlib.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 + +//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 0000000000..0b073cc654 --- /dev/null +++ b/apps/mavlink/orb_listener.c @@ -0,0 +1,644 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include + +#include "waypoints.h" +#include "mavlink_log.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" + +extern bool gcs_link; + +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; + + /* 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 second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = raw.gyro_counter; + } + + if (mag_counter != raw.magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = raw.magnetometer_counter; + } + + if (baro_counter != raw.baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = raw.baro_counter; + } + + if (gcs_link) + 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); + + 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); + + if (gcs_link) + /* 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); + + if (gcs_link) + /* 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); + + if (gcs_link) + 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; + + if (gcs_link) + 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); + + if (gcs_link) + 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); + + if (gcs_link) + 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); + + if (gcs_link) + 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); + + if (gcs_link) + 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); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "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, 200); /* 5Hz 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, 200); /* 5Hz 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 */ + pthread_attr_setstacksize(&uorb_attr, 4096); + + 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 0000000000..f041e7c4cb --- /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 + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 0000000000..a4ff06a883 --- /dev/null +++ b/apps/mavlink/util.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 8ed80c8171..3d9e5750aa 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 #include #include #include +#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 c525f4553d..736d1f119f 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 -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/multirotor_att_control/Makefile b/apps/multirotor_att_control/Makefile old mode 100644 new mode 100755 diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 7d5821d3ff..dfb778fd0a 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -172,7 +172,7 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; - printf("thrust_rate=%8.4f\n",offboard_sp.p4); +// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { @@ -180,7 +180,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; - printf("thrust_att=%8.4f\n",offboard_sp.p4); +// printf("thrust_att=%8.4f\n",offboard_sp.p4); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -193,18 +193,18 @@ mc_thread_main(int argc, char *argv[]) if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; + //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); } if (state.flag_control_attitude_enabled) { att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller - /* set yaw rate */ - rates_sp.yaw = manual.yaw; + att_sp.yaw_body = att.yaw + manual.yaw * -2.0f; att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } @@ -212,6 +212,7 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); if (motor_test_mode) { + printf("testmode"); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; @@ -224,21 +225,15 @@ mc_thread_main(int argc, char *argv[]) } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - - - /* run attitude controller */ - if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { - multirotor_control_attitude(&att_sp, &att, NULL, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + if (state.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } + } if (state.flag_control_rates_enabled) { - float gyro[3] = {0.0f, 0.0f, 0.0f}; + float gyro[3]; /* get current rate setpoint */ bool rates_sp_valid = false; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 2129915d12..8c0e10fc3a 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -54,21 +54,15 @@ #include #include -// PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); -// PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); -// PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -// PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -// PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f); - -PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); /* 0.025 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); @@ -76,17 +70,11 @@ PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { - // float yaw_p; - // float yaw_i; - // float yaw_d; - // float yaw_awu; - // float yaw_lim; - - float yawrate_p; - float yawrate_i; - float yawrate_d; - float yawrate_awu; - float yawrate_lim; + float yaw_p; + float yaw_i; + float yaw_d; + float yaw_awu; + float yaw_lim; float att_p; float att_i; @@ -99,17 +87,11 @@ struct mc_att_control_params { }; struct mc_att_control_param_handles { - // param_t yaw_p; - // param_t yaw_i; - // param_t yaw_d; - // param_t yaw_awu; - // param_t yaw_lim; - - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_d; - param_t yawrate_awu; - param_t yawrate_lim; + param_t yaw_p; + param_t yaw_i; + param_t yaw_d; + param_t yaw_awu; + param_t yaw_lim; param_t att_p; param_t att_i; @@ -137,17 +119,11 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc static int parameters_init(struct mc_att_control_param_handles *h) { /* PID parameters */ - // h->yaw_p = param_find("MC_YAWPOS_P"); - // h->yaw_i = param_find("MC_YAWPOS_I"); - // h->yaw_d = param_find("MC_YAWPOS_D"); - // h->yaw_awu = param_find("MC_YAWPOS_AWU"); - // h->yaw_lim = param_find("MC_YAWPOS_LIM"); - - h->yawrate_p = param_find("MC_YAWRATE_P"); - h->yawrate_i = param_find("MC_YAWRATE_I"); - h->yawrate_d = param_find("MC_YAWRATE_D"); - h->yawrate_awu = param_find("MC_YAWRATE_AWU"); - h->yawrate_lim = param_find("MC_YAWRATE_LIM"); + h->yaw_p = param_find("MC_YAWPOS_P"); + h->yaw_i = param_find("MC_YAWPOS_I"); + h->yaw_d = param_find("MC_YAWPOS_D"); + h->yaw_awu = param_find("MC_YAWPOS_AWU"); + h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); @@ -163,17 +139,11 @@ static int parameters_init(struct mc_att_control_param_handles *h) static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) { - // param_get(h->yaw_p, &(p->yaw_p)); - // param_get(h->yaw_i, &(p->yaw_i)); - // param_get(h->yaw_d, &(p->yaw_d)); - // param_get(h->yaw_awu, &(p->yaw_awu)); - // param_get(h->yaw_lim, &(p->yaw_lim)); - - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_d, &(p->yawrate_d)); - param_get(h->yawrate_awu, &(p->yawrate_awu)); - param_get(h->yawrate_lim, &(p->yawrate_lim)); + param_get(h->yaw_p, &(p->yaw_p)); + param_get(h->yaw_i, &(p->yaw_i)); + param_get(h->yaw_d, &(p->yaw_d)); + param_get(h->yaw_awu, &(p->yaw_awu)); + param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); @@ -188,16 +158,21 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + static uint64_t last_input = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; last_run = hrt_absolute_time(); + if (last_input != att_sp->timestamp) { + last_input = att_sp->timestamp; + } + static int sensor_delay; + sensor_delay = hrt_absolute_time() - att->timestamp; static int motor_skip_counter = 0; - // static PID_t yaw_pos_controller; - static PID_t yaw_speed_controller; static PID_t pitch_controller; static PID_t roll_controller; @@ -211,114 +186,40 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - // pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, - // PID_MODE_DERIVATIV_SET, 154); - pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, - PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + p.att_lim, PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + p.att_lim, PID_MODE_DERIVATIV_SET); initialized = true; } /* load new parameters with lower rate */ - if (motor_skip_counter % 50 == 0) { + if (motor_skip_counter % 1000 == 0) { /* update parameters from storage */ parameters_update(&h, &p); + /* apply parameters */ - // pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu); - pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu); - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu); + printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); } /* calculate current control outputs */ - + /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff, - att->pitch, att->pitchspeed, deltaT); + rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff, + att->pitch, att->pitchspeed, deltaT); + /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, - att->roll, att->rollspeed, deltaT); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, + att->roll, att->rollspeed, deltaT); + /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); + rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)); - /* - * compensate the vertical loss of thrust - * when thrust plane has an angle. - * start with a factor of 1.0 (no change) - */ - float zcompensation = 1.0f; - - if (fabsf(att->roll) > 0.3f) { - zcompensation *= 1.04675160154f; - - } else { - zcompensation *= 1.0f / cosf(att->roll); - } - - if (fabsf(att->pitch) > 0.3f) { - zcompensation *= 1.04675160154f; - - } else { - zcompensation *= 1.0f / cosf(att->pitch); - } - - float motor_thrust = 0.0f; - - motor_thrust = att_sp->thrust; - - /* compensate thrust vector for roll / pitch contributions */ - motor_thrust *= zcompensation; - - /* limit yaw rate output */ - if (yaw_rate_control > p.yawrate_lim) { - yaw_rate_control = p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } - - if (yaw_rate_control < -p.yawrate_lim) { - yaw_rate_control = -p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } - - if (pitch_control > p.att_lim) { - pitch_control = p.att_lim; - pitch_controller.saturated = 1; - } - - if (pitch_control < -p.att_lim) { - pitch_control = -p.att_lim; - pitch_controller.saturated = 1; - } - - - if (roll_control > p.att_lim) { - roll_control = p.att_lim; - roll_controller.saturated = 1; - } - - if (roll_control < -p.att_lim) { - roll_control = -p.att_lim; - roll_controller.saturated = 1; - } - - if (actuators) { - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; - actuators->control[3] = motor_thrust; - } - - // XXX change yaw rate to yaw pos controller - if (rates_sp) { - rates_sp->roll = roll_control; - rates_sp->pitch = pitch_control; - rates_sp->yaw = yaw_rate_control; - rates_sp->thrust = motor_thrust; - } + rates_sp->thrust = att_sp->thrust; motor_skip_counter++; } diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 7532dffa26..42aa0ac636 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -1,8 +1,8 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Tobias Naegeli - * @author Lorenz Meier + * Author: Tobias Naegeli + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,11 @@ /** * @file multirotor_rate_control.c + * * Implementation of rate controller + * + * @author Tobias Naegeli + * @author Lorenz Meier */ #include "multirotor_rate_control.h" @@ -48,18 +52,20 @@ #include #include #include +#include #include -// PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */ -// PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f); -// PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f); -// PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */ -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f); /**< roughly < 500 deg/s limit */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { @@ -144,10 +150,17 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { - static float roll_control_last=0; - static float pitch_control_last=0; + static float roll_control_last = 0; + static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + static uint64_t last_input = 0; + + float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; + if (last_input != rate_sp->timestamp) { + last_input = rate_sp->timestamp; + } + last_run = hrt_absolute_time(); static int motor_skip_counter = 0; @@ -168,21 +181,34 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p); + // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", + // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); } /* calculate current control outputs */ /* control pitch (forward) output */ + float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + /* increase resilience to faulty control inputs */ + if (isfinite(pitch_control)) { + pitch_control_last = pitch_control; + } else { + pitch_control = 0.0f; + warnx("rej. NaN ctrl pitch"); + } - float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last); - pitch_control_last=pitch_control; /* control roll (left/right) output */ + float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); + /* increase resilience to faulty control inputs */ + if (isfinite(roll_control)) { + roll_control_last = roll_control; + } else { + roll_control = 0.0f; + warnx("rej. NaN ctrl roll"); + } - float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last); - roll_control_last=roll_control; /* control yaw rate */ - float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] ); + float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c index 41f207b7e4..8c6951b63c 100644 --- a/apps/px4/tests/test_hrt.c +++ b/apps/px4/tests/test_hrt.c @@ -51,7 +51,7 @@ #include #include -#include +#include #include diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 22b8d82ee6..36225386c2 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -90,6 +90,11 @@ static void usage(const char *reason); static int file_exist(const char *filename); +/** + * Print the current status. + */ +static void print_sdlog_status(); + /** * Create folder for current logging session. */ @@ -103,6 +108,14 @@ usage(const char *reason) errx(1, "usage: sdlog {start|stop|status} [-p ]\n\n"); } +// XXX turn this into a C++ class +unsigned sensor_combined_bytes = 0; +unsigned actuator_outputs_bytes = 0; +unsigned actuator_controls_bytes = 0; +unsigned sysvector_bytes = 0; +unsigned blackbox_file_bytes = 0; +uint64_t starttime = 0; + /** * The sd log deamon app only briefly exists to start * the background job. The stack size assigned in the @@ -131,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); } @@ -145,7 +157,7 @@ int sdlog_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tsdlog is running\n"); + print_sdlog_status(); } else { printf("\tsdlog not started\n"); } @@ -194,7 +206,7 @@ int create_logfolder(char* folder_path) { int sdlog_thread_main(int argc, char *argv[]) { - printf("[sdlog] starting\n"); + warnx("starting\n"); if (file_exist(mountpoint) != OK) { errx(1, "logging mount point %s not present, exiting.", mountpoint); @@ -202,25 +214,20 @@ 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; - unsigned sensor_combined_bytes = 0; int actuator_outputs_file = -1; - unsigned actuator_outputs_bytes = 0; int actuator_controls_file = -1; - unsigned actuator_controls_bytes = 0; int sysvector_file = -1; - unsigned sysvector_bytes = 0; FILE *gpsfile; - unsigned blackbox_file_bytes = 0; FILE *blackbox_file; // FILE *vehiclefile; char path_buf[64] = ""; // string to hold the path to the sensorfile - printf("[sdlog] logging to directory %s\n", folder_path); + warnx("logging to directory %s\n", folder_path); /* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined"); @@ -347,14 +354,14 @@ int sdlog_thread_main(int argc, char *argv[]) { /* --- GLOBAL POSITION --- */ /* subscribe to ORB for global position */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); fds[fdsc_count].fd = subs.global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GPS POSITION --- */ /* subscribe to ORB for global position */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -378,7 +385,7 @@ int sdlog_thread_main(int argc, char *argv[]) { int poll_count = 0; - uint64_t starttime = hrt_absolute_time(); + starttime = hrt_absolute_time(); while (!thread_should_exit) { @@ -484,6 +491,7 @@ int sdlog_thread_main(int argc, char *argv[]) { float vbat; float adc[3]; float local_pos[3]; + int32_t gps_pos[3]; } sysvector = { .timestamp = buf.raw.timestamp, .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, @@ -497,7 +505,8 @@ int sdlog_thread_main(int argc, char *argv[]) { buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, .vbat = buf.raw.battery_voltage_v, .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, - .local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z} + .local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, + .gps_pos = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt} }; #pragma pack(pop) @@ -506,13 +515,11 @@ int sdlog_thread_main(int argc, char *argv[]) { usleep(10000); } - unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; - float mebibytes = bytes / 1024.0f / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; + fsync(sysvector_file); - printf("[sdlog] wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); + print_sdlog_status(); - printf("[sdlog] exiting.\n"); + warnx("exiting.\n"); close(sensorfile); close(actuator_outputs_file); @@ -525,6 +532,15 @@ int sdlog_thread_main(int argc, char *argv[]) { return 0; } +void print_sdlog_status() +{ + unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; + float mebibytes = bytes / 1024.0f / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; + + warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); +} + /** * @return 0 if file exists */ diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 52316993e8..5e8c5746c5 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -52,6 +52,10 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 0.0f); + PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index b84b58406f..3e204662a3 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,7 @@ /** * @file sensors.cpp + * @author Lorenz Meier * * Sensor readout process. */ @@ -178,6 +179,7 @@ private: float gyro_offset[3]; float mag_offset[3]; + float mag_scale[3]; float accel_offset[3]; float accel_scale[3]; @@ -209,6 +211,7 @@ private: param_t accel_offset[3]; param_t accel_scale[3]; param_t mag_offset[3]; + param_t mag_scale[3]; param_t rc_map_roll; param_t rc_map_pitch; @@ -414,6 +417,10 @@ Sensors::Sensors() : _parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF"); _parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF"); + _parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE"); + _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); + _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); /* fetch initial parameter values */ @@ -537,6 +544,10 @@ Sensors::parameters_update() param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0])); param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1])); param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2])); + /* mag scaling */ + param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_scale[0])); + param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_scale[1])); + param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_scale[2])); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -798,11 +809,11 @@ Sensors::parameter_update_poll(bool forced) fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale = { _parameters.mag_offset[0], - 1.0f, + _parameters.mag_scale[0], _parameters.mag_offset[1], - 1.0f, + _parameters.mag_scale[1], _parameters.mag_offset[2], - 1.0f, + _parameters.mag_scale[2], }; if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) warn("WARNING: failed to set scale / offsets for mag"); @@ -993,6 +1004,7 @@ Sensors::task_main() * do advertisements */ struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.battery_voltage_v = BAT_VOL_INITIAL; raw.adc_voltage_v[0] = 0.9f; diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index ce46d01cc8..bc467bfa3b 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -68,10 +68,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; @@ -79,13 +79,195 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub /* conscious mix of double and float trig function to maximize speed and efficiency */ float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - if (theta < M_PI_F) { - theta += 2.0f * M_PI_F; - } - - if (theta > M_PI_F) { - theta -= 2.0f * M_PI_F; - } + theta = _wrapPI(theta); return theta; -} \ No newline at end of file +} + +// Additional functions - @author Doug Weibel + +__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +{ +// This function returns the distance to the nearest point on the track line. Distance is positive if current +// position is right of the track and negative if left of the track as seen from a point on the track line +// headed towards the end point. + + crosstrack_error_s return_var; + float dist_to_end; + float bearing_end; + float bearing_track; + float bearing_diff; + + return_var.error = true; // Set error flag, cleared when valid result calculated. + return_var.past_end = false; + return_var.distance = 0.0f; + return_var.bearing = 0.0f; + + // Return error if arguments are bad + if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; + + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + bearing_diff = bearing_track - bearing_end; + bearing_diff = _wrapPI(bearing_diff); + + // Return past_end = true if past end point of line + if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + return_var.past_end = true; + return_var.error = false; + return return_var; + } + + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + return_var.distance = (dist_to_end)*sin(bearing_diff); + if(sin(bearing_diff) >=0 ) { + return_var.bearing = _wrapPI(bearing_track - M_PI_2_F); + } else { + return_var.bearing = _wrapPI(bearing_track + M_PI_2_F); + } + return_var.error = false; + + return return_var; + +} + + +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) +{ + // This function returns the distance to the nearest point on the track arc. Distance is positive if current + // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and + // headed towards the end point. + crosstrack_error_s return_var; + + // Determine if the current position is inside or outside the sector between the line from the center + // to the arc start and the line from the center to the arc end + float bearing_sector_start; + float bearing_sector_end; + float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + bool in_sector; + + return_var.error = true; // Set error flag, cleared when valid result calculated. + return_var.past_end = false; + return_var.distance = 0.0f; + return_var.bearing = 0.0f; + + // Return error if arguments are bad + if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; + + + if(arc_sweep >= 0) { + bearing_sector_start = arc_start_bearing; + bearing_sector_end = arc_start_bearing + arc_sweep; + if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F; + } else { + bearing_sector_end = arc_start_bearing; + bearing_sector_start = arc_start_bearing - arc_sweep; + if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; + } + in_sector = false; + + // Case where sector does not span zero + if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + + // Case where sector does span zero + if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true; + + // If in the sector then calculate distance and bearing to closest point + if(in_sector) { + return_var.past_end = false; + float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + + if(dist_to_center <= radius) { + return_var.distance = radius - dist_to_center; + return_var.bearing = bearing_now + M_PI_F; + } else { + return_var.distance = dist_to_center - radius; + return_var.bearing = bearing_now; + } + + // If out of the sector then calculate dist and bearing to start or end point + } else { + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile + float start_disp_x = radius * sin(arc_start_bearing); + float start_disp_y = radius * cos(arc_start_bearing); + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep)); + float lon_start = lon_now + start_disp_x/111111.0d; + float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d; + float lon_end = lon_now + end_disp_x/111111.0d; + float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d; + float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + if(dist_to_start < dist_to_end) { + return_var.distance = dist_to_start; + return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { + return_var.past_end = true; + return_var.distance = dist_to_end; + return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + } + + } + + return_var.bearing = _wrapPI(return_var.bearing); + return_var.error = false; + return return_var; +} + +float _wrapPI(float bearing) +{ + + while (bearing > M_PI_F) { + bearing = bearing - M_TWOPI_F; + } + while (bearing <= -M_PI_F) { + bearing = bearing + M_TWOPI_F; + } + return bearing; +} + +float _wrap2PI(float bearing) +{ + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + return bearing; +} + +float _wrap180(float bearing) +{ + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + return bearing; +} + +float _wrap360(float bearing) +{ + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + return bearing; +} + + + \ No newline at end of file diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index 0e86b3599d..205afc79e6 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -42,8 +42,31 @@ * @author Thomas Gubler * @author Julian Oes * @author Lorenz Meier + * Additional functions - @author Doug Weibel */ + + +#include + +typedef struct { + bool error; // Flag that the calculation failed + bool past_end; // Flag indicating we are past the end of the line/arc segment + float distance; // Distance in meters to closest point on line/arc + float bearing; // Bearing in radians to closest point on line/arc +} crosstrack_error_s; __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +// + +__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); + +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + +float _wrap180(float bearing); +float _wrap360(float bearing); +float _wrapPI(float bearing); +float _wrap2PI(float bearing); \ No newline at end of file diff --git a/apps/systemlib/getopt_long.h b/apps/systemlib/getopt_long.h index 3e51550a60..61e8e76f31 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/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index cff5e6bbe2..f9b50d030b 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -43,12 +43,13 @@ #include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - uint8_t mode) + float limit, uint8_t mode) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->intmax = intmax; + pid->limit = limit; pid->mode = mode; pid->count = 0; pid->saturated = 0; @@ -58,7 +59,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->error_previous = 0; pid->integral = 0; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { int ret = 0; @@ -85,8 +86,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } else { ret = 1; } + + if (isfinite(limit)) { + pid->limit = limit; + } else { + ret = 1; + } - // pid->limit = limit; return ret; } @@ -122,72 +128,48 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float i, d; pid->sp = sp; + + // Calculated current error value float error = pid->sp - val; - - if (pid->saturated && (pid->integral * error > 0)) { - //Output is saturated and the integral would get bigger (positive or negative) - i = pid->integral; - - //Reset saturation. If we are still saturated this will be set again at output limit check. - pid->saturated = 0; - - } else { - i = pid->integral + (error * dt); - } - - // Anti-Windup. Needed if we don't use the saturation above. - if (pid->intmax != 0.0f) { - if (i > pid->intmax) { - pid->integral = pid->intmax; - - } else if (i < -pid->intmax) { - - pid->integral = -pid->intmax; - - } else { - pid->integral = i; - } - } - - if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; - - } else if (pid->mode == PID_MODE_DERIVATIV_SET) { - d = -val_dot; - - } else { - d = 0.0f; - } - - if (pid->kd == 0.0f) { - d = 0.0f; - } - - if (pid->ki == 0.0f) { - i = 0; - } - - float p; - - if (pid->kp == 0.0f) { - p = 0.0f; - } else { - p = error; - } - - if (isfinite(error)) { + + if (isfinite(error)) { // Why is this necessary? DEW pid->error_previous = error; } + // Calculate or measured current error derivative + + if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / dt; + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { + d = -val_dot; + } else { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabs(i) > pid->intmax ) + { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + } else { + if (!isfinite(i)) { + i = 0; + } + pid->integral = i; + pid->saturated = 0; + } + + // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); + if (output > pid->limit) output = pid->limit; + if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { pid->last_output = output; } - if (!isfinite(pid->integral)) { - pid->integral = 0; - } return pid->last_output; } diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index 098b2bef81..b51afef9ef 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -49,6 +49,8 @@ #define PID_MODE_DERIVATIV_CALC 0 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ #define PID_MODE_DERIVATIV_SET 1 +// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) +#define PID_MODE_DERIVATIV_NONE 9 typedef struct { float kp; @@ -65,8 +67,8 @@ typedef struct { uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index 8c18477533..0324500acf 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -77,33 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */ + uint64_t timestamp; /**< Timestamp in microseconds since boot */ - int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */ - uint16_t gyro_counter; /**< Number of raw measurments taken LOGME */ - float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */ + int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ + uint16_t gyro_counter; /**< Number of raw measurments taken */ + float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */ - uint32_t accelerometer_counter; /**< Number of raw acc measurements taken LOGME */ - float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */ + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ + uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ + float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ - int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */ - float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */ + int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint32_t magnetometer_counter; /**< Number of raw mag measurements taken LOGME */ + uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ - float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */ - float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */ - float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */ - float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */ - float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */ - uint32_t baro_counter; /**< Number of raw baro measurements taken LOGME */ - uint32_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ - bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ + float baro_alt_meter; /**< Altitude, already temp. comp. */ + float baro_temp_celcius; /**< Temperature in degrees celsius */ + float battery_voltage_v; /**< Battery voltage in volts, filtered */ + float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 */ + uint32_t baro_counter; /**< Number of raw baro measurements taken */ + uint32_t battery_voltage_counter; /**< Number of voltage measurements taken */ + bool battery_voltage_valid; /**< True if battery voltage can be measured */ }; diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h index 172c37d0fa..82ff46ad26 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. diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 65868ef89a..0e27c32640 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 242, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 1, 234, 73, 181, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 242, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 1, 234, 73, 181, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h index 52d12f3c0a..be1ca4c831 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Fri Sep 14 11:04:09 2012" +#define MAVLINK_BUILD_DATE "Thu Oct 18 13:36:48 2012" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index 4db1a89844..b7f57785b6 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 242, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 242, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -482,6 +482,9 @@ enum MAV_SEVERITY #include "./mavlink_msg_vision_speed_estimate.h" #include "./mavlink_msg_vicon_position_estimate.h" #include "./mavlink_msg_highres_imu.h" +#include "./mavlink_msg_battery_status.h" +#include "./mavlink_msg_setpoint_8dof.h" +#include "./mavlink_msg_setpoint_6dof.h" #include "./mavlink_msg_memory_vect.h" #include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_named_value_float.h" diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h new file mode 100644 index 0000000000..c78fb4f31f --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h @@ -0,0 +1,320 @@ +// MESSAGE BATTERY_STATUS PACKING + +#define MAVLINK_MSG_ID_BATTERY_STATUS 147 + +typedef struct __mavlink_battery_status_t +{ + uint16_t voltage_cell_1; ///< Battery voltage of cell 1, in millivolts (1 = 1 millivolt) + uint16_t voltage_cell_2; ///< Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell + uint16_t voltage_cell_3; ///< Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell + uint16_t voltage_cell_4; ///< Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell + uint16_t voltage_cell_5; ///< Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell + uint16_t voltage_cell_6; ///< Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + uint8_t accu_id; ///< Accupack ID + int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery +} mavlink_battery_status_t; + +#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 16 +#define MAVLINK_MSG_ID_147_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ + "BATTERY_STATUS", \ + 9, \ + { { "voltage_cell_1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_battery_status_t, voltage_cell_1) }, \ + { "voltage_cell_2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_battery_status_t, voltage_cell_2) }, \ + { "voltage_cell_3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_battery_status_t, voltage_cell_3) }, \ + { "voltage_cell_4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_battery_status_t, voltage_cell_4) }, \ + { "voltage_cell_5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_battery_status_t, voltage_cell_5) }, \ + { "voltage_cell_6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_battery_status_t, voltage_cell_6) }, \ + { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_battery_status_t, current_battery) }, \ + { "accu_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_battery_status_t, accu_id) }, \ + { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 15, offsetof(mavlink_battery_status_t, battery_remaining) }, \ + } \ +} + + +/** + * @brief Pack a battery_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param accu_id Accupack ID + * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) + * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint16_t(buf, 0, voltage_cell_1); + _mav_put_uint16_t(buf, 2, voltage_cell_2); + _mav_put_uint16_t(buf, 4, voltage_cell_3); + _mav_put_uint16_t(buf, 6, voltage_cell_4); + _mav_put_uint16_t(buf, 8, voltage_cell_5); + _mav_put_uint16_t(buf, 10, voltage_cell_6); + _mav_put_int16_t(buf, 12, current_battery); + _mav_put_uint8_t(buf, 14, accu_id); + _mav_put_int8_t(buf, 15, battery_remaining); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16); +#else + mavlink_battery_status_t packet; + packet.voltage_cell_1 = voltage_cell_1; + packet.voltage_cell_2 = voltage_cell_2; + packet.voltage_cell_3 = voltage_cell_3; + packet.voltage_cell_4 = voltage_cell_4; + packet.voltage_cell_5 = voltage_cell_5; + packet.voltage_cell_6 = voltage_cell_6; + packet.current_battery = current_battery; + packet.accu_id = accu_id; + packet.battery_remaining = battery_remaining; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 16, 42); +} + +/** + * @brief Pack a battery_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param accu_id Accupack ID + * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) + * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t accu_id,uint16_t voltage_cell_1,uint16_t voltage_cell_2,uint16_t voltage_cell_3,uint16_t voltage_cell_4,uint16_t voltage_cell_5,uint16_t voltage_cell_6,int16_t current_battery,int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint16_t(buf, 0, voltage_cell_1); + _mav_put_uint16_t(buf, 2, voltage_cell_2); + _mav_put_uint16_t(buf, 4, voltage_cell_3); + _mav_put_uint16_t(buf, 6, voltage_cell_4); + _mav_put_uint16_t(buf, 8, voltage_cell_5); + _mav_put_uint16_t(buf, 10, voltage_cell_6); + _mav_put_int16_t(buf, 12, current_battery); + _mav_put_uint8_t(buf, 14, accu_id); + _mav_put_int8_t(buf, 15, battery_remaining); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16); +#else + mavlink_battery_status_t packet; + packet.voltage_cell_1 = voltage_cell_1; + packet.voltage_cell_2 = voltage_cell_2; + packet.voltage_cell_3 = voltage_cell_3; + packet.voltage_cell_4 = voltage_cell_4; + packet.voltage_cell_5 = voltage_cell_5; + packet.voltage_cell_6 = voltage_cell_6; + packet.current_battery = current_battery; + packet.accu_id = accu_id; + packet.battery_remaining = battery_remaining; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 42); +} + +/** + * @brief Encode a battery_status struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) +{ + return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->battery_remaining); +} + +/** + * @brief Send a battery_status message + * @param chan MAVLink channel to send the message + * + * @param accu_id Accupack ID + * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) + * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell + * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint16_t(buf, 0, voltage_cell_1); + _mav_put_uint16_t(buf, 2, voltage_cell_2); + _mav_put_uint16_t(buf, 4, voltage_cell_3); + _mav_put_uint16_t(buf, 6, voltage_cell_4); + _mav_put_uint16_t(buf, 8, voltage_cell_5); + _mav_put_uint16_t(buf, 10, voltage_cell_6); + _mav_put_int16_t(buf, 12, current_battery); + _mav_put_uint8_t(buf, 14, accu_id); + _mav_put_int8_t(buf, 15, battery_remaining); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, 16, 42); +#else + mavlink_battery_status_t packet; + packet.voltage_cell_1 = voltage_cell_1; + packet.voltage_cell_2 = voltage_cell_2; + packet.voltage_cell_3 = voltage_cell_3; + packet.voltage_cell_4 = voltage_cell_4; + packet.voltage_cell_5 = voltage_cell_5; + packet.voltage_cell_6 = voltage_cell_6; + packet.current_battery = current_battery; + packet.accu_id = accu_id; + packet.battery_remaining = battery_remaining; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, 16, 42); +#endif +} + +#endif + +// MESSAGE BATTERY_STATUS UNPACKING + + +/** + * @brief Get field accu_id from battery_status message + * + * @return Accupack ID + */ +static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field voltage_cell_1 from battery_status message + * + * @return Battery voltage of cell 1, in millivolts (1 = 1 millivolt) + */ +static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field voltage_cell_2 from battery_status message + * + * @return Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell + */ +static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field voltage_cell_3 from battery_status message + * + * @return Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell + */ +static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field voltage_cell_4 from battery_status message + * + * @return Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell + */ +static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field voltage_cell_5 from battery_status message + * + * @return Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell + */ +static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field voltage_cell_6 from battery_status message + * + * @return Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + */ +static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field current_battery from battery_status message + * + * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + */ +static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field battery_remaining from battery_status message + * + * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + */ +static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 15); +} + +/** + * @brief Decode a battery_status message into a struct + * + * @param msg The message to decode + * @param battery_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* msg, mavlink_battery_status_t* battery_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + battery_status->voltage_cell_1 = mavlink_msg_battery_status_get_voltage_cell_1(msg); + battery_status->voltage_cell_2 = mavlink_msg_battery_status_get_voltage_cell_2(msg); + battery_status->voltage_cell_3 = mavlink_msg_battery_status_get_voltage_cell_3(msg); + battery_status->voltage_cell_4 = mavlink_msg_battery_status_get_voltage_cell_4(msg); + battery_status->voltage_cell_5 = mavlink_msg_battery_status_get_voltage_cell_5(msg); + battery_status->voltage_cell_6 = mavlink_msg_battery_status_get_voltage_cell_6(msg); + battery_status->current_battery = mavlink_msg_battery_status_get_current_battery(msg); + battery_status->accu_id = mavlink_msg_battery_status_get_accu_id(msg); + battery_status->battery_remaining = mavlink_msg_battery_status_get_battery_remaining(msg); +#else + memcpy(battery_status, _MAV_PAYLOAD(msg), 16); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h new file mode 100644 index 0000000000..fec38a6a48 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h @@ -0,0 +1,276 @@ +// MESSAGE SETPOINT_6DOF PACKING + +#define MAVLINK_MSG_ID_SETPOINT_6DOF 149 + +typedef struct __mavlink_setpoint_6dof_t +{ + float trans_x; ///< Translational Component in x + float trans_y; ///< Translational Component in y + float trans_z; ///< Translational Component in z + float rot_x; ///< Rotational Component in x + float rot_y; ///< Rotational Component in y + float rot_z; ///< Rotational Component in z + uint8_t target_system; ///< System ID +} mavlink_setpoint_6dof_t; + +#define MAVLINK_MSG_ID_SETPOINT_6DOF_LEN 25 +#define MAVLINK_MSG_ID_149_LEN 25 + + + +#define MAVLINK_MESSAGE_INFO_SETPOINT_6DOF { \ + "SETPOINT_6DOF", \ + 7, \ + { { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_6dof_t, trans_x) }, \ + { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_6dof_t, trans_y) }, \ + { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_6dof_t, trans_z) }, \ + { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_6dof_t, rot_x) }, \ + { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_6dof_t, rot_y) }, \ + { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_6dof_t, rot_z) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_setpoint_6dof_t, target_system) }, \ + } \ +} + + +/** + * @brief Pack a setpoint_6dof message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param trans_x Translational Component in x + * @param trans_y Translational Component in y + * @param trans_z Translational Component in z + * @param rot_x Rotational Component in x + * @param rot_y Rotational Component in y + * @param rot_z Rotational Component in z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, trans_x); + _mav_put_float(buf, 4, trans_y); + _mav_put_float(buf, 8, trans_z); + _mav_put_float(buf, 12, rot_x); + _mav_put_float(buf, 16, rot_y); + _mav_put_float(buf, 20, rot_z); + _mav_put_uint8_t(buf, 24, target_system); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); +#else + mavlink_setpoint_6dof_t packet; + packet.trans_x = trans_x; + packet.trans_y = trans_y; + packet.trans_z = trans_z; + packet.rot_x = rot_x; + packet.rot_y = rot_y; + packet.rot_z = rot_z; + packet.target_system = target_system; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF; + return mavlink_finalize_message(msg, system_id, component_id, 25, 15); +} + +/** + * @brief Pack a setpoint_6dof message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param trans_x Translational Component in x + * @param trans_y Translational Component in y + * @param trans_z Translational Component in z + * @param rot_x Rotational Component in x + * @param rot_y Rotational Component in y + * @param rot_z Rotational Component in z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,float trans_x,float trans_y,float trans_z,float rot_x,float rot_y,float rot_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, trans_x); + _mav_put_float(buf, 4, trans_y); + _mav_put_float(buf, 8, trans_z); + _mav_put_float(buf, 12, rot_x); + _mav_put_float(buf, 16, rot_y); + _mav_put_float(buf, 20, rot_z); + _mav_put_uint8_t(buf, 24, target_system); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); +#else + mavlink_setpoint_6dof_t packet; + packet.trans_x = trans_x; + packet.trans_y = trans_y; + packet.trans_z = trans_z; + packet.rot_x = rot_x; + packet.rot_y = rot_y; + packet.rot_z = rot_z; + packet.target_system = target_system; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 15); +} + +/** + * @brief Encode a setpoint_6dof struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param setpoint_6dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) +{ + return mavlink_msg_setpoint_6dof_pack(system_id, component_id, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); +} + +/** + * @brief Send a setpoint_6dof message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param trans_x Translational Component in x + * @param trans_y Translational Component in y + * @param trans_z Translational Component in z + * @param rot_x Rotational Component in x + * @param rot_y Rotational Component in y + * @param rot_z Rotational Component in z + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, trans_x); + _mav_put_float(buf, 4, trans_y); + _mav_put_float(buf, 8, trans_z); + _mav_put_float(buf, 12, rot_x); + _mav_put_float(buf, 16, rot_y); + _mav_put_float(buf, 20, rot_z); + _mav_put_uint8_t(buf, 24, target_system); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, 25, 15); +#else + mavlink_setpoint_6dof_t packet; + packet.trans_x = trans_x; + packet.trans_y = trans_y; + packet.trans_z = trans_z; + packet.rot_x = rot_x; + packet.rot_y = rot_y; + packet.rot_z = rot_z; + packet.target_system = target_system; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)&packet, 25, 15); +#endif +} + +#endif + +// MESSAGE SETPOINT_6DOF UNPACKING + + +/** + * @brief Get field target_system from setpoint_6dof message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_setpoint_6dof_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field trans_x from setpoint_6dof message + * + * @return Translational Component in x + */ +static inline float mavlink_msg_setpoint_6dof_get_trans_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field trans_y from setpoint_6dof message + * + * @return Translational Component in y + */ +static inline float mavlink_msg_setpoint_6dof_get_trans_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field trans_z from setpoint_6dof message + * + * @return Translational Component in z + */ +static inline float mavlink_msg_setpoint_6dof_get_trans_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field rot_x from setpoint_6dof message + * + * @return Rotational Component in x + */ +static inline float mavlink_msg_setpoint_6dof_get_rot_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field rot_y from setpoint_6dof message + * + * @return Rotational Component in y + */ +static inline float mavlink_msg_setpoint_6dof_get_rot_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field rot_z from setpoint_6dof message + * + * @return Rotational Component in z + */ +static inline float mavlink_msg_setpoint_6dof_get_rot_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a setpoint_6dof message into a struct + * + * @param msg The message to decode + * @param setpoint_6dof C-struct to decode the message contents into + */ +static inline void mavlink_msg_setpoint_6dof_decode(const mavlink_message_t* msg, mavlink_setpoint_6dof_t* setpoint_6dof) +{ +#if MAVLINK_NEED_BYTE_SWAP + setpoint_6dof->trans_x = mavlink_msg_setpoint_6dof_get_trans_x(msg); + setpoint_6dof->trans_y = mavlink_msg_setpoint_6dof_get_trans_y(msg); + setpoint_6dof->trans_z = mavlink_msg_setpoint_6dof_get_trans_z(msg); + setpoint_6dof->rot_x = mavlink_msg_setpoint_6dof_get_rot_x(msg); + setpoint_6dof->rot_y = mavlink_msg_setpoint_6dof_get_rot_y(msg); + setpoint_6dof->rot_z = mavlink_msg_setpoint_6dof_get_rot_z(msg); + setpoint_6dof->target_system = mavlink_msg_setpoint_6dof_get_target_system(msg); +#else + memcpy(setpoint_6dof, _MAV_PAYLOAD(msg), 25); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h new file mode 100644 index 0000000000..bc761be08d --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h @@ -0,0 +1,320 @@ +// MESSAGE SETPOINT_8DOF PACKING + +#define MAVLINK_MSG_ID_SETPOINT_8DOF 148 + +typedef struct __mavlink_setpoint_8dof_t +{ + float val1; ///< Value 1 + float val2; ///< Value 2 + float val3; ///< Value 3 + float val4; ///< Value 4 + float val5; ///< Value 5 + float val6; ///< Value 6 + float val7; ///< Value 7 + float val8; ///< Value 8 + uint8_t target_system; ///< System ID +} mavlink_setpoint_8dof_t; + +#define MAVLINK_MSG_ID_SETPOINT_8DOF_LEN 33 +#define MAVLINK_MSG_ID_148_LEN 33 + + + +#define MAVLINK_MESSAGE_INFO_SETPOINT_8DOF { \ + "SETPOINT_8DOF", \ + 9, \ + { { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_8dof_t, val1) }, \ + { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_8dof_t, val2) }, \ + { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_8dof_t, val3) }, \ + { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_8dof_t, val4) }, \ + { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_8dof_t, val5) }, \ + { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_8dof_t, val6) }, \ + { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_setpoint_8dof_t, val7) }, \ + { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_setpoint_8dof_t, val8) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_setpoint_8dof_t, target_system) }, \ + } \ +} + + +/** + * @brief Pack a setpoint_8dof message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param val1 Value 1 + * @param val2 Value 2 + * @param val3 Value 3 + * @param val4 Value 4 + * @param val5 Value 5 + * @param val6 Value 6 + * @param val7 Value 7 + * @param val8 Value 8 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_float(buf, 0, val1); + _mav_put_float(buf, 4, val2); + _mav_put_float(buf, 8, val3); + _mav_put_float(buf, 12, val4); + _mav_put_float(buf, 16, val5); + _mav_put_float(buf, 20, val6); + _mav_put_float(buf, 24, val7); + _mav_put_float(buf, 28, val8); + _mav_put_uint8_t(buf, 32, target_system); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); +#else + mavlink_setpoint_8dof_t packet; + packet.val1 = val1; + packet.val2 = val2; + packet.val3 = val3; + packet.val4 = val4; + packet.val5 = val5; + packet.val6 = val6; + packet.val7 = val7; + packet.val8 = val8; + packet.target_system = target_system; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF; + return mavlink_finalize_message(msg, system_id, component_id, 33, 241); +} + +/** + * @brief Pack a setpoint_8dof message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param val1 Value 1 + * @param val2 Value 2 + * @param val3 Value 3 + * @param val4 Value 4 + * @param val5 Value 5 + * @param val6 Value 6 + * @param val7 Value 7 + * @param val8 Value 8 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,float val1,float val2,float val3,float val4,float val5,float val6,float val7,float val8) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_float(buf, 0, val1); + _mav_put_float(buf, 4, val2); + _mav_put_float(buf, 8, val3); + _mav_put_float(buf, 12, val4); + _mav_put_float(buf, 16, val5); + _mav_put_float(buf, 20, val6); + _mav_put_float(buf, 24, val7); + _mav_put_float(buf, 28, val8); + _mav_put_uint8_t(buf, 32, target_system); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); +#else + mavlink_setpoint_8dof_t packet; + packet.val1 = val1; + packet.val2 = val2; + packet.val3 = val3; + packet.val4 = val4; + packet.val5 = val5; + packet.val6 = val6; + packet.val7 = val7; + packet.val8 = val8; + packet.target_system = target_system; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 241); +} + +/** + * @brief Encode a setpoint_8dof struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param setpoint_8dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) +{ + return mavlink_msg_setpoint_8dof_pack(system_id, component_id, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); +} + +/** + * @brief Send a setpoint_8dof message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param val1 Value 1 + * @param val2 Value 2 + * @param val3 Value 3 + * @param val4 Value 4 + * @param val5 Value 5 + * @param val6 Value 6 + * @param val7 Value 7 + * @param val8 Value 8 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_float(buf, 0, val1); + _mav_put_float(buf, 4, val2); + _mav_put_float(buf, 8, val3); + _mav_put_float(buf, 12, val4); + _mav_put_float(buf, 16, val5); + _mav_put_float(buf, 20, val6); + _mav_put_float(buf, 24, val7); + _mav_put_float(buf, 28, val8); + _mav_put_uint8_t(buf, 32, target_system); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, 33, 241); +#else + mavlink_setpoint_8dof_t packet; + packet.val1 = val1; + packet.val2 = val2; + packet.val3 = val3; + packet.val4 = val4; + packet.val5 = val5; + packet.val6 = val6; + packet.val7 = val7; + packet.val8 = val8; + packet.target_system = target_system; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)&packet, 33, 241); +#endif +} + +#endif + +// MESSAGE SETPOINT_8DOF UNPACKING + + +/** + * @brief Get field target_system from setpoint_8dof message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_setpoint_8dof_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field val1 from setpoint_8dof message + * + * @return Value 1 + */ +static inline float mavlink_msg_setpoint_8dof_get_val1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field val2 from setpoint_8dof message + * + * @return Value 2 + */ +static inline float mavlink_msg_setpoint_8dof_get_val2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field val3 from setpoint_8dof message + * + * @return Value 3 + */ +static inline float mavlink_msg_setpoint_8dof_get_val3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field val4 from setpoint_8dof message + * + * @return Value 4 + */ +static inline float mavlink_msg_setpoint_8dof_get_val4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field val5 from setpoint_8dof message + * + * @return Value 5 + */ +static inline float mavlink_msg_setpoint_8dof_get_val5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field val6 from setpoint_8dof message + * + * @return Value 6 + */ +static inline float mavlink_msg_setpoint_8dof_get_val6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field val7 from setpoint_8dof message + * + * @return Value 7 + */ +static inline float mavlink_msg_setpoint_8dof_get_val7(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field val8 from setpoint_8dof message + * + * @return Value 8 + */ +static inline float mavlink_msg_setpoint_8dof_get_val8(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a setpoint_8dof message into a struct + * + * @param msg The message to decode + * @param setpoint_8dof C-struct to decode the message contents into + */ +static inline void mavlink_msg_setpoint_8dof_decode(const mavlink_message_t* msg, mavlink_setpoint_8dof_t* setpoint_8dof) +{ +#if MAVLINK_NEED_BYTE_SWAP + setpoint_8dof->val1 = mavlink_msg_setpoint_8dof_get_val1(msg); + setpoint_8dof->val2 = mavlink_msg_setpoint_8dof_get_val2(msg); + setpoint_8dof->val3 = mavlink_msg_setpoint_8dof_get_val3(msg); + setpoint_8dof->val4 = mavlink_msg_setpoint_8dof_get_val4(msg); + setpoint_8dof->val5 = mavlink_msg_setpoint_8dof_get_val5(msg); + setpoint_8dof->val6 = mavlink_msg_setpoint_8dof_get_val6(msg); + setpoint_8dof->val7 = mavlink_msg_setpoint_8dof_get_val7(msg); + setpoint_8dof->val8 = mavlink_msg_setpoint_8dof_get_val8(msg); + setpoint_8dof->target_system = mavlink_msg_setpoint_8dof_get_target_system(msg); +#else + memcpy(setpoint_8dof, _MAV_PAYLOAD(msg), 33); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index 2bcaac77a0..8b51c09590 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -3780,6 +3780,179 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_battery_status_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 17651, + 17755, + 17859, + 175, + 242, + }; + mavlink_battery_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.voltage_cell_1 = packet_in.voltage_cell_1; + packet1.voltage_cell_2 = packet_in.voltage_cell_2; + packet1.voltage_cell_3 = packet_in.voltage_cell_3; + packet1.voltage_cell_4 = packet_in.voltage_cell_4; + packet1.voltage_cell_5 = packet_in.voltage_cell_5; + packet1.voltage_cell_6 = packet_in.voltage_cell_6; + packet1.current_battery = packet_in.current_battery; + packet1.accu_id = packet_in.accu_id; + packet1.battery_remaining = packet_in.battery_remaining; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_battery_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_battery_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_battery_status_pack(system_id, component_id, &msg , packet1.accu_id , packet1.voltage_cell_1 , packet1.voltage_cell_2 , packet1.voltage_cell_3 , packet1.voltage_cell_4 , packet1.voltage_cell_5 , packet1.voltage_cell_6 , packet1.current_battery , packet1.battery_remaining ); + mavlink_msg_battery_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.accu_id , packet1.voltage_cell_1 , packet1.voltage_cell_2 , packet1.voltage_cell_3 , packet1.voltage_cell_4 , packet1.voltage_cell_5 , packet1.voltage_cell_6 , packet1.current_battery , packet1.battery_remaining ); + mavlink_msg_battery_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i #include -#include #include #include #include @@ -277,10 +276,5 @@ int nsh_archinitialize(void) } #endif - /* configure the tone generator */ -#ifdef CONFIG_TONE_ALARM - tone_alarm_init(); -#endif - return OK; }