fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 05:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'daregger_rate_control' of github.com:PX4/Firmware into calibration

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-19 00:39:06 +02:00
parent 97726fa679 5ec5754f26
commit d3ae83cb22
26 changed files with 1227 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
-1
View File
@@ -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);
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+27 -32
View File
@@ -71,11 +71,10 @@ __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,
@@ -88,21 +87,10 @@ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 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};
@@ -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};
@@ -293,13 +281,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
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+=1;
offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) {
initialized = true;
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
printf("pipapo %d\n",(int)(gyro_offsets[2]*1000) );
}
} else {
@@ -316,9 +304,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
sensor_last_timestamp[0] = raw.timestamp;
}
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];
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];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -362,7 +350,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
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*/)
if (!const_initialized && dt < 0.05 && dt > 0.005)
{
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
@@ -388,16 +376,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
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));
/* 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 */
@@ -425,7 +416,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
// 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);
// }
printcounter++;
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+3 -3
View File
@@ -61,9 +61,9 @@ 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-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-1f);
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, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+6 -6
View File
@@ -50,7 +50,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
b_u1 = -1;
}
y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
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;
@@ -60,7 +60,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
y = 0.0F;
}
} else {
y = (real32_T)atan2(u0, u1);
y = (real32_T)atan2f(u0, u1);
}
return y;
@@ -776,12 +776,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
Rot_matrix[6 + i] = z_n_b[i];
}
/* 'attitudeKalmanfilter:193' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
/* 'attitudeKalmanfilter:194' theta=-asin(Rot_matrix(1,3)); */
/* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* '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)asin(Rot_matrix[6]);
eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
}
apps/commander/commander.c
+34 -23
View File
@@ -292,10 +292,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
struct sensor_combined_s raw;
/* 30 seconds */
uint64_t calibration_interval = 30 * 1000 * 1000;
uint64_t calibration_interval = 45 * 1000 * 1000;
unsigned int calibration_counter = 0;
float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN};
/*
* 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, 0);
@@ -322,8 +328,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
uint64_t axis_deadline = hrt_absolute_time();
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
const char axislabels[3] = { 'X', 'Y', 'Z'};
int axis_index = 0;
const char axislabels[3] = { 'Z', 'X', 'Y'};
int axis_index = -1;
while (hrt_absolute_time() < calibration_deadline) {
@@ -331,18 +337,21 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
/* user guidance */
if (hrt_absolute_time() > axis_deadline &&
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);
ioctl(buzzer, TONE_SET_ALARM, 2);
axis_deadline += calibration_interval / 3;
axis_index++;
}
if (!(axis_index < 3)) {
continue;
break;
}
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
@@ -358,26 +367,26 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
/* get min/max values */
/* ignore other axes */
if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) {
mag_min[axis_index] = raw.magnetometer_ga[axis_index];
if (raw.magnetometer_ga[0] < mag_min[0]) {
mag_min[0] = raw.magnetometer_ga[0];
}
else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) {
mag_max[axis_index] = raw.magnetometer_ga[axis_index];
else if (raw.magnetometer_ga[0] > mag_max[0]) {
mag_max[0] = raw.magnetometer_ga[0];
}
// 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 {
@@ -840,7 +849,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");
ioctl(buzzer, TONE_SET_ALARM, 2);
do_accel_calibration(status_pub, &current_status);
ioctl(buzzer, TONE_SET_ALARM, 2);
mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
apps/mavlink/mavlink.c
+4 -4
View File
@@ -1,4 +1,4 @@
/****************************************************************************
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
@@ -576,10 +576,10 @@ 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, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
/* 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 */
apps/mavlink/mavlink_receiver.c
+7 -11
View File
@@ -225,28 +225,20 @@ handle_message(mavlink_message_t *msg)
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;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
ml_armed = true;
break;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
@@ -259,7 +251,6 @@ handle_message(mavlink_message_t *msg)
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;
@@ -487,6 +478,11 @@ 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, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_t thread;
apps/mavlink/orb_listener.c
+6 -15
View File
@@ -141,12 +141,6 @@ l_sensor_combined(struct listener *l)
last_sensor_timestamp = raw.timestamp;
/* send raw imu data */
// mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, raw.accelerometer_raw[0],
// raw.accelerometer_raw[1], raw.accelerometer_raw[2], raw.gyro_raw[0],
// raw.gyro_raw[1], raw.gyro_raw[2], raw.magnetometer_raw[0],
// raw.magnetometer_raw[1], raw.magnetometer_raw[2]);
/* mark individual fields as changed */
uint16_t fields_updated = 0;
static unsigned accel_counter = 0;
@@ -161,19 +155,19 @@ l_sensor_combined(struct listener *l)
}
if (gyro_counter != raw.gyro_counter) {
/* mark first three dimensions as changed */
/* 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 first three dimensions as changed */
/* 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 first three dimensions as changed */
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
baro_counter = raw.baro_counter;
}
@@ -187,8 +181,6 @@ l_sensor_combined(struct listener *l)
raw.baro_pres_mbar, 0 /* no diff pressure yet */,
raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated);
/* send pressure */
//mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, raw.timestamp / 1000, raw.baro_pres_mbar, raw.baro_alt_meter, raw.baro_temp_celcius * 100);
sensors_raw_counter++;
}
@@ -559,12 +551,12 @@ uorb_receive_start(void)
/* --- SENSORS RAW VALUE --- */
mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
/* rate limit set externally based on interface speed, set a basic default here */
orb_set_interval(mavlink_subs.sensor_sub, 100); /* 10Hz updates */
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, 100); /* 10Hz updates */
orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */
/* --- GPS VALUE --- */
mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -631,8 +623,7 @@ uorb_receive_start(void)
pthread_attr_init(&uorb_attr);
/* Set stack size, needs more than 8000 bytes */
/* XXX verify, should not need anything like this much unless MAVLink really sucks */
pthread_attr_setstacksize(&uorb_attr, 8192);
pthread_attr_setstacksize(&uorb_attr, 4096);
pthread_t thread;
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
apps/multirotor_att_control/multirotor_att_control_main.c
+1 -3
View File
@@ -204,9 +204,7 @@ mc_thread_main(int argc, char *argv[])
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, clean up
/* 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();
}
apps/multirotor_att_control/multirotor_attitude_control.c
+6 -5
View File
@@ -235,20 +235,21 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
// 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("delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
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));
}
/* calculate current control outputs */
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
att->pitch, att->pitchspeed, deltaT)*1/10.0f;
att->pitch, att->pitchspeed, deltaT);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT)*1/10.0f;
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)*1/10.0f;
rates_sp->yaw= deltaT*p.yaw_p*atan2f(sinf(att->yaw-att_sp->yaw_body),cos(att->yaw-att_sp->yaw_body));
rates_sp->yaw= deltaT * p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body));
apps/multirotor_att_control/multirotor_rate_control.c
+17 -4
View File
@@ -52,6 +52,7 @@
#include <math.h>
#include <systemlib/pid/pid.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <arch/board/up_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */
@@ -153,6 +154,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
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;
@@ -173,31 +181,36 @@ 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\n",
(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 * deltaT *((rate_sp->pitch)*p.attrate_lim-rates[1])-p.attrate_d*(pitch_control_last);
// XXX fix this line so that deltaT is also applied to the D term
float pitch_control = p.attrate_p * deltaT * (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\n");
}
/* control roll (left/right) output */
float roll_control = p.attrate_p * deltaT * ((rate_sp->roll)*p.attrate_lim-rates[0])-p.attrate_d*(roll_control_last);
// XXX fix this line so that deltaT is also applied to the D term
float roll_control = p.attrate_p * deltaT * (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\n");
}
/* control yaw rate */
float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2]);
float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)-rates[2]);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
apps/sensors/sensors.cpp
+1
View File
@@ -1004,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;
mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/ardupilotmega/version.h
+1 -1
View File
@@ -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
mavlink/include/mavlink/v1.0/common/common.h
+6 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
+320
View File
@@ -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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h
+276
View File
@@ -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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h
+320
View File
@@ -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
}
mavlink/include/mavlink/v1.0/common/testsuite.h
+176
View File
@@ -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<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_battery_status_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_battery_status_send(MAVLINK_COMM_1 , 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(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_setpoint_8dof(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_setpoint_8dof_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
185.0,
213.0,
101,
};
mavlink_setpoint_8dof_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.val1 = packet_in.val1;
packet1.val2 = packet_in.val2;
packet1.val3 = packet_in.val3;
packet1.val4 = packet_in.val4;
packet1.val5 = packet_in.val5;
packet1.val6 = packet_in.val6;
packet1.val7 = packet_in.val7;
packet1.val8 = packet_in.val8;
packet1.target_system = packet_in.target_system;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_8dof_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_setpoint_8dof_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_8dof_pack(system_id, component_id, &msg , packet1.target_system , packet1.val1 , packet1.val2 , packet1.val3 , packet1.val4 , packet1.val5 , packet1.val6 , packet1.val7 , packet1.val8 );
mavlink_msg_setpoint_8dof_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.val1 , packet1.val2 , packet1.val3 , packet1.val4 , packet1.val5 , packet1.val6 , packet1.val7 , packet1.val8 );
mavlink_msg_setpoint_8dof_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<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_setpoint_8dof_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_8dof_send(MAVLINK_COMM_1 , packet1.target_system , packet1.val1 , packet1.val2 , packet1.val3 , packet1.val4 , packet1.val5 , packet1.val6 , packet1.val7 , packet1.val8 );
mavlink_msg_setpoint_8dof_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_setpoint_6dof(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_setpoint_6dof_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
77,
};
mavlink_setpoint_6dof_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.trans_x = packet_in.trans_x;
packet1.trans_y = packet_in.trans_y;
packet1.trans_z = packet_in.trans_z;
packet1.rot_x = packet_in.rot_x;
packet1.rot_y = packet_in.rot_y;
packet1.rot_z = packet_in.rot_z;
packet1.target_system = packet_in.target_system;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_6dof_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_setpoint_6dof_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_6dof_pack(system_id, component_id, &msg , packet1.target_system , packet1.trans_x , packet1.trans_y , packet1.trans_z , packet1.rot_x , packet1.rot_y , packet1.rot_z );
mavlink_msg_setpoint_6dof_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.trans_x , packet1.trans_y , packet1.trans_z , packet1.rot_x , packet1.rot_y , packet1.rot_z );
mavlink_msg_setpoint_6dof_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<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_setpoint_6dof_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_setpoint_6dof_send(MAVLINK_COMM_1 , packet1.target_system , packet1.trans_x , packet1.trans_y , packet1.trans_z , packet1.rot_x , packet1.rot_y , packet1.rot_z );
mavlink_msg_setpoint_6dof_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_memory_vect(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
@@ -4138,6 +4311,9 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
mavlink_test_vision_speed_estimate(system_id, component_id, last_msg);
mavlink_test_vicon_position_estimate(system_id, component_id, last_msg);
mavlink_test_highres_imu(system_id, component_id, last_msg);
mavlink_test_battery_status(system_id, component_id, last_msg);
mavlink_test_setpoint_8dof(system_id, component_id, last_msg);
mavlink_test_setpoint_6dof(system_id, component_id, last_msg);
mavlink_test_memory_vect(system_id, component_id, last_msg);
mavlink_test_debug_vect(system_id, component_id, last_msg);
mavlink_test_named_value_float(system_id, component_id, last_msg);
mavlink/include/mavlink/v1.0/common/version.h
+1 -1
View File
@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Fri Sep 14 11:05:17 2012"
#define MAVLINK_BUILD_DATE "Thu Oct 18 13:37:22 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/matrixpilot/version.h
+1 -1
View File
@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Fri Sep 14 10:42:05 2012"
#define MAVLINK_BUILD_DATE "Thu Oct 18 13:37:22 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/pixhawk/version.h
+1 -1
View File
@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Fri Sep 14 10:41:07 2012"
#define MAVLINK_BUILD_DATE "Thu Oct 18 13:37:02 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/sensesoar/version.h
+1 -1
View File
@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Fri Sep 14 10:41:18 2012"
#define MAVLINK_BUILD_DATE "Thu Oct 18 13:37:13 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101