From 2bb1d17c7e312e6f60bcd6e8f1ac6698fe623060 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 7 Oct 2012 14:46:26 -0600 Subject: [PATCH 01/40] Changes to the PID controller. Adds "limit" to the parameter set. Implements an output limit where the output magnitude is limited by the parameter value "limit". Also changes the integrator saturation such that the integrator is not updated (added to) if either updating it will cause the integrator values magnitude to exceed "intmax" or if the output magnitude would exceed "limit" with an updated integrator value. Arbitrary large limit values were hard coded into multirotor_attitude_control.c. These should be changed to parametric values or something sensible. This commit will temporarily break fixedwing_control.c. A following commit will repair it along with significant changes to the inner loop control. This commit has been tested to compile with fixedwing_control.c temporarily removed. No other testing has been completed. --- .../multirotor_attitude_control.c | 16 +-- apps/systemlib/pid/pid.c | 100 +++++++----------- apps/systemlib/pid/pid.h | 6 +- 3 files changed, 53 insertions(+), 69 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 2129915d12..cb34209b1c 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -212,13 +212,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s 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); + // 90.0f, PID_MODE_DERIVATIV_SET, 154); // Arbitrarily large limit added pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, - PID_MODE_DERIVATIV_SET); + 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added initialized = true; } @@ -228,10 +228,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* 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); + // pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, 90.0f); + pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, 90.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f); } /* calculate current control outputs */ 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); From 4fea0a3fc15346efc366aadf1d80697664b6f3f0 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 7 Oct 2012 14:50:07 -0600 Subject: [PATCH 02/40] This commit changes the inner loop control structures for fixed wing attitude control. Nested rate and angle loops are used with limits on both the rate setpoint A simple outer navigation loop is retained for navigation control. This will be replaced later. The pitch set point is hard coded to zero. Pitch stabilization should work. This commit compiles, but needs further testing. --- apps/fixedwing_control/fixedwing_control.c | 160 ++++++++++++++++----- 1 file changed, 121 insertions(+), 39 deletions(-) diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index ad08247e10..a693971c97 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_ROLL_RATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// Suggested value of FW_ROLL_RATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_ROLL_RATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLL_RATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLL_RATE_LIMIT, 0.7f); // Roll rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_LIMIT, 0.7f); // Roll angle limit in radians + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// Suggested value of FW_PITCH_RATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_LIMIT, 0.35f); // Pitch rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_LIMIT, 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 roll_rate_p; + float roll_rate_i; + float roll_rate_awu; + float roll_rate_limit; + float roll_angle_p; + float roll_angle_limit; + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_awu; + float pitch_rate_limit; + float pitch_angle_p; + float pitch_angle_limit; }; 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 roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_awu; + param_t roll_rate_limit; + param_t roll_angle_p; + param_t roll_angle_limit; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_awu; + param_t pitch_rate_limit; + param_t pitch_angle_p; + param_t pitch_angle_limit; }; -// 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.roll_rate_p, p.roll_rate_i, 0.0f, p.roll_rate_awu, + p.roll_rate_limit,PID_MODE_DERIVATIV_NONE); + PID_t roll_angle_controller; + pid_init(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, 0.0f, + p.roll_angle_limit,PID_MODE_DERIVATIV_NONE); + + PID_t pitch_rate_controller; + pid_init(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, p.pitch_rate_awu, + p.pitch_rate_limit,PID_MODE_DERIVATIV_NONE); + PID_t pitch_angle_controller; + pid_init(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, 0.0f, + p.pitch_angle_limit,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] = { @@ -254,8 +301,15 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (loopcounter % 20 == 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.roll_rate_p, p.roll_rate_i, 0.0f, + p.roll_rate_awu, p.roll_rate_limit); + pid_set_parameters(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, + 0.0f, p.roll_angle_limit); + pid_set_parameters(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, + p.pitch_rate_awu, p.pitch_rate_limit); + pid_set_parameters(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, + 0.0f, p.pitch_angle_limit); + pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); } /* if position updated, run position control loop */ @@ -359,14 +413,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 +511,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->roll_rate_p = param_find("FW_ROLL_RATE_P"); + h->roll_rate_i = param_find(";FW_ROLL_RATE_I"); + h->roll_rate_awu = param_find("FW_ROLL_RATE_AWU"); + h->roll_rate_limit = param_find("FW_ROLL_RATE_LIMIT"); + h->roll_angle_p = param_find("FW_ROLL_ANGLE_P"); + h->roll_angle_limit = param_find("FW_ROLL_ANGLE_LIMIT"); + h->pitch_rate_p = param_find("FW_PITCH_RATE_P"); + h->pitch_rate_i = param_find("FW_PITCH_RATE_I"); + h->pitch_rate_awu = param_find("FW_PITCH_RATE_AWU"); + h->pitch_rate_limit = param_find("FW_PITCH_RATE_LIMIT"); + h->pitch_angle_p = param_find("FW_PITCH_ANGLE_P"); + h->pitch_angle_p = param_find("FW_PITCH_ANGLE_LIMIT"); + 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->roll_rate_p, &(p->roll_rate_p)); + param_get(h->roll_rate_i, &(p->roll_rate_i)); + param_get(h->roll_rate_awu, &(p->roll_rate_awu)); + param_get(h->roll_rate_limit, &(p->roll_rate_limit)); + param_get(h->roll_angle_p, &(p->roll_angle_p)); + param_get(h->roll_angle_limit, &(p->roll_angle_limit)); + param_get(h->pitch_rate_p, &(p->pitch_rate_p)); + param_get(h->pitch_rate_i, &(p->pitch_rate_i)); + param_get(h->pitch_rate_awu, &(p->pitch_rate_awu)); + param_get(h->pitch_rate_limit, &(p->pitch_rate_limit)); + param_get(h->pitch_angle_p, &(p->pitch_angle_p)); + param_get(h->pitch_angle_limit, &(p->pitch_angle_limit)); + return OK; } From 77e6375920df67344e895e669dd2a641a7b87b6e Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Mon, 8 Oct 2012 14:14:43 -0600 Subject: [PATCH 03/40] Change parameter names in FW control to fit within MAVLink parameter name size limit --- apps/fixedwing_control/fixedwing_control.c | 160 +++++++++++---------- 1 file changed, 81 insertions(+), 79 deletions(-) diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index a693971c97..8629c2f110 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -88,53 +88,53 @@ static void usage(const char *reason); * */ // Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); // Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_ROLL_RATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_LIMIT, 0.7f); // Roll rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_LIMIT, 0.7f); // Roll angle limit in radians +// 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_PITCH_RATE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); // Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_PITCH_RATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_PITCH_RATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RATE_LIMIT, 0.35f); // Pitch rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_LIMIT, 0.35f); // Pitch angle limit in radians +// 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_rate_p; - float roll_rate_i; - float roll_rate_awu; - float roll_rate_limit; - float roll_angle_p; - float roll_angle_limit; - float pitch_rate_p; - float pitch_rate_i; - float pitch_rate_awu; - float pitch_rate_limit; - float pitch_angle_p; - float pitch_angle_limit; + 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_rate_p; - param_t roll_rate_i; - param_t roll_rate_awu; - param_t roll_rate_limit; - param_t roll_angle_p; - param_t roll_angle_limit; - param_t pitch_rate_p; - param_t pitch_rate_i; - param_t pitch_rate_awu; - param_t pitch_rate_limit; - param_t pitch_angle_p; - param_t pitch_angle_limit; + 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; }; @@ -260,18 +260,18 @@ int fixedwing_control_thread_main(int argc, char *argv[]) // 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.roll_rate_p, p.roll_rate_i, 0.0f, p.roll_rate_awu, - p.roll_rate_limit,PID_MODE_DERIVATIV_NONE); + 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_angle_p, 0.0f, 0.0f, 0.0f, - p.roll_angle_limit,PID_MODE_DERIVATIV_NONE); + 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.pitch_rate_p, p.pitch_rate_i, 0.0f, p.pitch_rate_awu, - p.pitch_rate_limit,PID_MODE_DERIVATIV_NONE); + 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_angle_p, 0.0f, 0.0f, 0.0f, - p.pitch_angle_limit,PID_MODE_DERIVATIV_NONE); + 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, @@ -298,18 +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_rate_controller, p.roll_rate_p, p.roll_rate_i, 0.0f, - p.roll_rate_awu, p.roll_rate_limit); - pid_set_parameters(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, - 0.0f, p.roll_angle_limit); - pid_set_parameters(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, - p.pitch_rate_awu, p.pitch_rate_limit); - pid_set_parameters(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, - 0.0f, p.pitch_angle_limit); + 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 */ @@ -512,36 +514,36 @@ static int att_parameters_init(struct fw_att_control_param_handles *h) { /* PID parameters */ - h->roll_rate_p = param_find("FW_ROLL_RATE_P"); - h->roll_rate_i = param_find(";FW_ROLL_RATE_I"); - h->roll_rate_awu = param_find("FW_ROLL_RATE_AWU"); - h->roll_rate_limit = param_find("FW_ROLL_RATE_LIMIT"); - h->roll_angle_p = param_find("FW_ROLL_ANGLE_P"); - h->roll_angle_limit = param_find("FW_ROLL_ANGLE_LIMIT"); - h->pitch_rate_p = param_find("FW_PITCH_RATE_P"); - h->pitch_rate_i = param_find("FW_PITCH_RATE_I"); - h->pitch_rate_awu = param_find("FW_PITCH_RATE_AWU"); - h->pitch_rate_limit = param_find("FW_PITCH_RATE_LIMIT"); - h->pitch_angle_p = param_find("FW_PITCH_ANGLE_P"); - h->pitch_angle_p = param_find("FW_PITCH_ANGLE_LIMIT"); + 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_rate_p, &(p->roll_rate_p)); - param_get(h->roll_rate_i, &(p->roll_rate_i)); - param_get(h->roll_rate_awu, &(p->roll_rate_awu)); - param_get(h->roll_rate_limit, &(p->roll_rate_limit)); - param_get(h->roll_angle_p, &(p->roll_angle_p)); - param_get(h->roll_angle_limit, &(p->roll_angle_limit)); - param_get(h->pitch_rate_p, &(p->pitch_rate_p)); - param_get(h->pitch_rate_i, &(p->pitch_rate_i)); - param_get(h->pitch_rate_awu, &(p->pitch_rate_awu)); - param_get(h->pitch_rate_limit, &(p->pitch_rate_limit)); - param_get(h->pitch_angle_p, &(p->pitch_angle_p)); - param_get(h->pitch_angle_limit, &(p->pitch_angle_limit)); + 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; } From 87ce36eef37340901710d5a3f25b31b97f3fba3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 10 Oct 2012 17:47:28 +0200 Subject: [PATCH 04/40] Fixed logging, merged --- apps/commander/commander.c | 20 +++++++++++---- apps/sdlog/sdlog.c | 51 +++++++++++++++++++++++++------------- 2 files changed, 49 insertions(+), 22 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c569a6aa4a..78fac5c221 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -296,7 +296,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) int calibration_interval_ms = 30 * 1000; unsigned int calibration_counter = 0; - float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN}; float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); @@ -353,6 +353,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } } + usleep(200000); + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); /* disable calibration mode */ @@ -377,7 +379,15 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - 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]); + if (!isfinite(mag_offset[0]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) + { + mavlink_log_info(mavlink_fd, "[commander] mag cal aborted: NaN"); + } + + //char buf[52]; + + //sprintf(buf, "mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); + //mavlink_log_info(mavlink_fd, buf); /* announce and set new offset */ @@ -412,7 +422,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: auto-save of params to EEPROM failed"); } - mavlink_log_info(mavlink_fd, "[commander] magnetometer calibration finished"); + mavlink_log_info(mavlink_fd, "[commander] mag cal finished"); close(sub_sensor_combined); } @@ -519,7 +529,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; @@ -954,7 +964,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; diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 22b8d82ee6..c02cf2fcfe 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 @@ -145,7 +158,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 +207,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); @@ -206,21 +219,16 @@ int sdlog_thread_main(int argc, char *argv[]) { /* 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 +355,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 +386,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 +492,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 +506,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 +516,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 +533,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 */ From 7c20e666815a2259235db7b45db86fd4ce3ec999 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Oct 2012 17:59:15 -0700 Subject: [PATCH 05/40] Refactor and start tidying up the MAVLink app. --- apps/mavlink/mavlink.c | 1351 ++------------------------ apps/mavlink/mavlink_bridge_header.h | 18 +- apps/mavlink/mavlink_hil.h | 57 ++ apps/mavlink/mavlink_receiver.c | 495 ++++++++++ apps/mavlink/missionlib.c | 190 ++++ apps/mavlink/missionlib.h | 52 + apps/mavlink/orb_listener.c | 640 ++++++++++++ apps/mavlink/orb_topics.h | 98 ++ apps/mavlink/util.h | 54 + apps/mavlink/waypoints.c | 14 +- apps/mavlink/waypoints.h | 1 - apps/uORB/uORB.h | 2 + 12 files changed, 1687 insertions(+), 1285 deletions(-) create mode 100644 apps/mavlink/mavlink_hil.h create mode 100644 apps/mavlink/mavlink_receiver.c create mode 100644 apps/mavlink/missionlib.c create mode 100644 apps/mavlink/missionlib.h create mode 100644 apps/mavlink/orb_listener.c create mode 100644 apps/mavlink/orb_topics.h create mode 100644 apps/mavlink/util.h diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 0592d03775..3a1365c662 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -58,30 +58,18 @@ #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,63 +77,47 @@ 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; - -/** 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; +bool mavlink_hil_enabled = false; +/* buffer for message strings */ static char mavlink_message_string[51] = {0}; +/* protocol interface */ +static int uart; static int baudrate = 57600; /* interface mode */ @@ -154,207 +126,19 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; -static struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int armed_sub; - int actuators_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int spg_sub; - int debug_key_value; - bool initialized; -} mavlink_subs = { - .sensor_sub = 0, - .att_sub = 0, - .global_pos_sub = 0, - .act_0_sub = 0, - .act_1_sub = 0, - .act_2_sub = 0, - .act_3_sub = 0, - .gps_sub = 0, - .man_control_sp_sub = 0, - .armed_sub = 0, - .actuators_sub = 0, - .local_pos_sub = 0, - .spa_sub = 0, - .spl_sub = 0, - .spg_sub = 0, - .debug_key_value = 0, - .initialized = false -}; - -static struct mavlink_publications { - orb_advert_t offboard_control_sp_pub; - orb_advert_t vicon_position_pub; -} mavlink_pubs = { - .offboard_control_sp_pub = -1, - .vicon_position_pub = -1 -}; - - -/* 3: Define waypoint helper functions */ -void mavlink_wpm_send_message(mavlink_message_t *msg); -void mavlink_wpm_send_gcs_string(const char *string); -uint64_t mavlink_wpm_get_system_timestamp(void); -int mavlink_missionlib_send_message(mavlink_message_t *msg); -int mavlink_missionlib_send_gcs_string(const char *string); -uint64_t mavlink_missionlib_get_system_timestamp(void); - -void handleMessage(mavlink_message_t *msg); static void mavlink_update_system(void); +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -/** - * Enable / disable Hardware in the Loop simulation mode. - */ -int set_hil_on_off(bool hil_enabled); - -/** - * Translate the custom state into standard mavlink modes and state. - */ -void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const struct actuator_armed_s *actuator, uint8_t *mavlink_state, uint8_t *mavlink_mode); - -int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - -/* 4: Include waypoint protocol */ -#include "waypoints.h" -mavlink_wpm_storage *wpm; - - -#include "mavlink_parameters.h" /** * Print the usage */ static void usage(const char *reason); -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -int mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - int writelen = write(uart, missionlib_msg_buf, len); - if (writelen != len) { - return 1; - } else { - return 0; - } -} - -int mavlink_missionlib_send_gcs_string(const char *string) -{ - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') - break; - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - return mavlink_missionlib_send_message(&msg); - } else { - return 1; - } -} - -/** - * Get system time since boot in microseconds - * - * @return the system time since boot in microseconds - */ -uint64_t mavlink_missionlib_get_system_timestamp() -{ - return hrt_absolute_time(); -} - -/** - * This callback is executed each time a waypoint changes. - * - * It publishes the vehicle_global_position_setpoint_s or the - * vehicle_local_position_setpoint_s topic, depending on the type of waypoint - */ -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) -{ - char buf[50] = {0}; - - /* Update controller setpoints */ - if (frame == (int)MAV_FRAME_GLOBAL) { - /* global, absolute waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = false; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - /* global, relative alt (in relation to HOME) waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = true; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { - /* local, absolute waypoint */ - struct vehicle_local_position_setpoint_s sp; - sp.x = param5_lat_x; - sp.y = param6_lon_y; - sp.z = param7_alt_z; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - /* Initialize publication if necessary */ - if (local_position_setpoint_pub < 0) { - local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); - } - sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - } - - mavlink_missionlib_send_gcs_string(buf); - printf("%s\n", buf); - //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); -} - -int set_hil_on_off(bool hil_enabled) +int +set_hil_on_off(bool hil_enabled) { int ret = OK; @@ -363,9 +147,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 +182,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 +190,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 +269,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 +276,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 +315,6 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma return ret; } -/** - * Listen for uORB topics and send via MAVLink. - * - * This pthread performs a blocking wait on selected - * uORB topics and sends them via MAVLink to other - * vehicles or a ground control station. - */ -static void *uorb_receiveloop(void *arg) -{ - /* obtain reference to task's subscriptions */ - struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg; - - /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); - - - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 25; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - - - union { - struct sensor_combined_s raw; - struct vehicle_attitude_s att; - struct vehicle_gps_position_s gps; - struct vehicle_local_position_setpoint_s local_sp; - struct vehicle_global_position_setpoint_s global_sp; - struct vehicle_attitude_setpoint_s att_sp; - struct actuator_outputs_s act_outputs; - struct manual_control_setpoint_s man_control; - struct actuator_controls_s actuators; - struct debug_key_value_s debug; - } buf; - - /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ - subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = subs->sensor_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ - subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(subs->att_sub, 100); - fds[fdsc_count].fd = subs->att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS VALUE --- */ - /* subscribe to ORB for attitude */ - subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(subs->gps_sub, 1000); /* 1Hz updates */ - fds[fdsc_count].fd = subs->gps_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- SYSTEM STATE --- */ - /* struct already globally allocated */ - /* subscribe to topic */ - int status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - fds[fdsc_count].fd = status_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- RC CHANNELS VALUE --- */ - /* struct already globally allocated */ - /* subscribe to ORB for global position */ - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(rc_sub, 100); /* 10Hz updates */ - fds[fdsc_count].fd = rc_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POS VALUE --- */ - /* struct already globally allocated and topic already subscribed */ - fds[fdsc_count].fd = subs->global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POS VALUE --- */ - /* struct and topic already globally subscribed */ - fds[fdsc_count].fd = subs->local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL SETPOINT VALUE --- */ - /* subscribe to ORB for local setpoint */ - /* struct already allocated */ - subs->spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - orb_set_interval(subs->spg_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spg_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL SETPOINT VALUE --- */ - /* subscribe to ORB for local setpoint */ - /* struct already allocated */ - subs->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(subs->spl_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spl_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ - subs->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(subs->spa_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spa_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- ACTUATOR OUTPUTS --- */ - subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs->act_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - fds[fdsc_count].fd = subs->act_1_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - fds[fdsc_count].fd = subs->act_2_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); - fds[fdsc_count].fd = subs->act_3_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- MAPPED MANUAL CONTROL INPUTS --- */ - subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - fds[fdsc_count].fd = subs->man_control_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR ARMED VALUE --- */ - /* subscribe to ORB for actuator armed */ - subs->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - fds[fdsc_count].fd = subs->armed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs->actuators_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- DEBUG VALUE OUTPUT --- */ - /* subscribe to ORB for debug value outputs */ - subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - fds[fdsc_count].fd = subs->debug_key_value; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* all subscriptions initialized, return success */ - subs->initialized = true; - - unsigned int sensors_raw_counter = 0; - unsigned int attitude_counter = 0; - unsigned int gps_counter = 0; - - /* WARNING: If you get the error message below, - * then the number of registered messages (fdsc) - * differs from the number of messages in the above list. - */ - if (fdsc_count > fdsc) { - fprintf(stderr, "[mavlink] WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; - - /* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ - uint64_t last_sensor_timestamp = 0; - - while (!thread_should_exit) { - - int poll_ret = poll(fds, fdsc_count, timeout); - - /* handle the poll result */ - if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); - } else if (poll_ret < 0) { - mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); - } else { - - int ifds = 0; - - /* --- SENSORS RAW VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw); - - last_sensor_timestamp = buf.raw.timestamp; - - /* send raw imu data */ - // mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0], - // buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], - // buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], - // buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); - - /* mark individual fields as changed */ - uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; - - if (accel_counter != buf.raw.accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = buf.raw.accelerometer_counter; - } - - if (gyro_counter != buf.raw.gyro_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = buf.raw.gyro_counter; - } - - if (mag_counter != buf.raw.magnetometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = buf.raw.magnetometer_counter; - } - - if (baro_counter != buf.raw.baro_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = buf.raw.baro_counter; - } - - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], - buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0], - buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2], - buf.raw.magnetometer_ga[0], - buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2], - buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */, - buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius, - fields_updated); - /* send pressure */ - //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100); - - sensors_raw_counter++; - } - - /* --- ATTITUDE VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att); - - /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); - - attitude_counter++; - } - - /* --- GPS VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps); - /* GPS position */ - mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible); - - if (buf.gps.satellite_info_available && (gps_counter % 4 == 0)) { - mavlink_msg_gps_status_send(MAVLINK_COMM_0, buf.gps.satellites_visible, buf.gps.satellite_prn, buf.gps.satellite_used, buf.gps.satellite_elevation, buf.gps.satellite_azimuth, buf.gps.satellite_snr); - } - - gps_counter++; - } - - /* --- SYSTEM STATUS --- */ - if (fds[ifds++].revents & POLLIN) { - /* immediately communicate state changes back to user */ - orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); - /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); - } - - /* --- RC CHANNELS --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw, - rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi); - } - - /* --- VEHICLE GLOBAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos); - uint64_t timestamp = global_pos.timestamp; - int32_t lat = global_pos.lat; - int32_t lon = global_pos.lon; - int32_t alt = (int32_t)(global_pos.alt*1000); - int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); - int16_t vx = (int16_t)(global_pos.vx * 100.0f); - int16_t vy = (int16_t)(global_pos.vy * 100.0f); - int16_t vz = (int16_t)(global_pos.vz * 100.0f); - /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); - - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, - relative_alt, vx, vy, vz, hdg); - } - - /* --- VEHICLE LOCAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), subs->local_pos_sub, &local_pos); - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, - local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz); - } - - /* --- VEHICLE GLOBAL SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position_setpoint), subs->spg_sub, &buf.global_sp); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - if (buf.global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat, - buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw); - } - - /* --- VEHICLE LOCAL SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), subs->spl_sub, &buf.local_sp); - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, - buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw); - } - - /* --- VEHICLE ATTITUDE SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp); - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, - buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); - } - - /* --- ACTUATOR OUTPUTS 0 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 0 /* port 0 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - - // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), - // 1 /* port 1 */, - // buf.act_outputs.output[ 8], - // buf.act_outputs.output[ 9], - // buf.act_outputs.output[10], - // buf.act_outputs.output[11], - // buf.act_outputs.output[12], - // buf.act_outputs.output[13], - // buf.act_outputs.output[14], - // buf.act_outputs.output[15]); - // } - } - - /* --- ACTUATOR OUTPUTS 1 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 3 /* port 3 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- ACTUATOR OUTPUTS 2 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 5 /* port 5 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- ACTUATOR OUTPUTS 3 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 7 /* port 7 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000, - buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0); - } - - /* --- ACTUATOR ARMED --- */ - if (fds[ifds++].revents & POLLIN) { - } - - /* --- ACTUATOR CONTROL --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators); - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl0 ", buf.actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]); - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - buf.actuators.control[0], - buf.actuators.control[1], - buf.actuators.control[2], - buf.actuators.control[3], - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } - } - - /* --- DEBUG KEY/VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug); - /* Enforce null termination */ - buf.debug.key[sizeof(buf.debug.key) - 1] = '\0'; - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.debug.key, buf.debug.value); - } - } - } - - return NULL; -} /**************************************************************************** * MAVLink text message logger @@ -1152,349 +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,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } +void +mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) +{ + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); +} + void mavlink_update_system(void) { static bool initialized = false; @@ -1631,22 +493,10 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { - wpm = &wpm_s; - - /* initialize global data structs */ - memset(&global_pos, 0, sizeof(global_pos)); - memset(&local_pos, 0, sizeof(local_pos)); - memset(&v_status, 0, sizeof(v_status)); - memset(&rc, 0, sizeof(rc)); - memset(&hil_attitude, 0, sizeof(hil_attitude)); - memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - memset(&vcmd, 0, sizeof(vcmd)); - /* 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. */ + /* create 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 */ @@ -1704,40 +554,19 @@ int mavlink_thread_main(int argc, char *argv[]) /* 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); @@ -1791,19 +620,14 @@ int mavlink_thread_main(int argc, char *argv[]) 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 +636,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 +665,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,8 +676,10 @@ int mavlink_thread_main(int argc, char *argv[]) usleep(10000); /* send one string at 10 Hz */ - mavlink_missionlib_send_gcs_string(mavlink_message_string); - mavlink_message_string[0] = '\0'; + if (mavlink_message_string[0] != '\0') { + mavlink_missionlib_send_gcs_string(mavlink_message_string); + mavlink_message_string[0] = '\0'; + } counter++; /* sleep 15 ms */ diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h index 26bc26fdfc..8d34c39243 100644 --- a/apps/mavlink/mavlink_bridge_header.h +++ b/apps/mavlink/mavlink_bridge_header.h @@ -62,28 +62,12 @@ */ extern mavlink_system_t mavlink_system; - -mqd_t gps_queue; -int uart; - - /** * @brief Send multiple chars (uint8_t) over a comm channel * * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 * @param ch Character to send */ -static inline void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, uint16_t length) -{ - ssize_t ret; - - if (chan == MAVLINK_COMM_0) { - ret = write(uart, ch, (size_t)(sizeof(uint8_t) * length)); - - if (ret != length) { - printf("[mavlink] Error: Written %u instead of %u\n", ret, length); - } - } -} +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h new file mode 100644 index 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_receiver.c b/apps/mavlink/mavlink_receiver.c new file mode 100644 index 0000000000..ad46c3edea --- /dev/null +++ b/apps/mavlink/mavlink_receiver.c @@ -0,0 +1,495 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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; + +static void +handle_message(mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + printf("[mavlink] Terminating .. \n"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + thread_should_exit = true; + + } else { + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } + /* publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + } + + if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + + f.timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + /* check if topic is advertised */ + if (flow_pub <= 0) { + flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &f); + } + } + + if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { + /* Set mode on request */ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = new_mode.custom_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + + /* Handle Vicon position estimates */ + if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); + + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + + if (vicon_position_pub <= 0) { + vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { + orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); + } + } + + /* Handle quadrotor motor setpoints */ + + if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + //printf("got message\n"); + //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); + + if (mavlink_system.sysid < 4) { + /* + * rate control mode - defined by MAVLink + */ + + uint8_t ml_mode = 0; + bool ml_armed = false; + +// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) { +// ml_armed = true; +// } + + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + + break; + case 1: + + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + case 2: + + + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } + + offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX; + //offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ; + + if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){ + ml_armed = false; + + } + + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = ml_mode; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + /* check if topic has to be advertised */ + if (offboard_control_sp_pub <= 0) { + offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { + /* Publish */ + orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); + } + + // /* change armed status if required */ + // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); + + // bool cmd_generated = false; + + // if (v_status.flag_control_offboard_enabled != cmd_armed) { + // vcmd.param1 = cmd_armed; + // vcmd.param2 = 0; + // vcmd.param3 = 0; + // vcmd.param4 = 0; + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // /* check if input has to be enabled */ + // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || + // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || + // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || + // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { + // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); + // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); + // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); + // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // if (cmd_generated) { + // /* check if topic is advertised */ + // if (cmd_pub <= 0) { + // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + // } else { + // /* create command */ + // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + // } + // } + } + } + + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + */ + + // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); + + if (mavlink_hil_enabled) { + + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { + + mavlink_hil_state_t hil_state; + mavlink_msg_hil_state_decode(msg, &hil_state); + + // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " + // "ROLL %i \n PITCH %i \n YAW %i \n" + // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", + // hil_state.lat/1000000, // 1e7 + // hil_state.lon/1000000, // 1e7 + // hil_state.alt/1000, // mm + // hil_state.roll, // float rad + // hil_state.pitch, // float rad + // hil_state.yaw, // float rad + // hil_state.rollspeed, // float rad/s + // hil_state.pitchspeed, // float rad/s + // hil_state.yawspeed); // float rad/s + + + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + hil_global_pos.vy = hil_state.vy / 100.0f; + hil_global_pos.vz = hil_state.vz / 100.0f; + + /* set timestamp and notify processes (broadcast) */ + hil_global_pos.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + + hil_attitude.roll = hil_state.roll; + hil_attitude.pitch = hil_state.pitch; + hil_attitude.yaw = hil_state.yaw; + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + /* set timestamp and notify processes (broadcast) */ + hil_attitude.counter++; + hil_attitude.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } + + if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + struct rc_channels_s rc_hil; + memset(&rc_hil, 0, sizeof(rc_hil)); + static orb_advert_t rc_pub = 0; + + rc_hil.timestamp = hrt_absolute_time(); + rc_hil.chan_count = 4; + rc_hil.chan[0].raw = 1500 + man.x / 2; + rc_hil.chan[1].raw = 1500 + man.y / 2; + rc_hil.chan[2].raw = 1500 + man.r / 2; + rc_hil.chan[3].raw = 1500 + man.z / 2; + + rc_hil.chan[0].scaled = man.x / 1000.0f; + rc_hil.chan[1].scaled = man.y / 1000.0f; + rc_hil.chan[2].scaled = man.r / 1000.0f; + rc_hil.chan[3].scaled = man.z / 1000.0f; + + struct manual_control_setpoint_s mc; + static orb_advert_t mc_pub = 0; + + mc.timestamp = rc_hil.timestamp; + mc.roll = man.x / 1000.0f; + mc.pitch = man.y / 1000.0f; + mc.yaw = man.r / 1000.0f; + mc.throttle = man.z / 1000.0f; + + /* fake RC channels with manual control input from simulator */ + + + if (rc_pub == 0) { + rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); + } else { + orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); + } + + if (mc_pub == 0) { + mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); + } else { + orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); + } + } + } +} + + +/** + * Receive data from UART. + */ +static void * +receive_thread(void *arg) +{ + int uart_fd = *((int*)arg); + + const int timeout = 1000; + uint8_t ch; + + mavlink_message_t msg; + + prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + + while (!thread_should_exit) { + + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + + if (poll(fds, 1, timeout) > 0) { + /* non-blocking read until buffer is empty */ + int nread = 0; + + do { + nread = read(uart_fd, &ch, 1); + + if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* handle generic messages and commands */ + handle_message(&msg); + + /* Handle packet with waypoint component */ + mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + + /* Handle packet with parameter component */ + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + } + } while (nread > 0); + } + } + + return NULL; +} + +pthread_t +receive_start(int uart) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + pthread_attr_setstacksize(&receiveloop_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + return thread; +} \ No newline at end of file diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c new file mode 100644 index 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..39358922df --- /dev/null +++ b/apps/mavlink/orb_listener.c @@ -0,0 +1,640 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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" + +struct vehicle_global_position_s global_pos; +struct vehicle_local_position_s local_pos; +struct vehicle_status_s v_status; +struct rc_channels_s rc; +struct actuator_armed_s armed; + +struct mavlink_subscriptions mavlink_subs; + +static int status_sub; +static int rc_sub; + +static unsigned int sensors_raw_counter; +static unsigned int attitude_counter; +static unsigned int gps_counter; + +/* + * Last sensor loop time + * some outputs are better timestamped + * with this "global" reference. + */ +static uint64_t last_sensor_timestamp; + +static void *uorb_receive_thread(void *arg); + +struct listener +{ + void (*callback)(struct listener *l); + int *subp; + uintptr_t arg; +}; + +static void l_sensor_combined(struct listener *l); +static void l_vehicle_attitude(struct listener *l); +static void l_vehicle_gps_position(struct listener *l); +static void l_vehicle_status(struct listener *l); +static void l_rc_channels(struct listener *l); +static void l_global_position(struct listener *l); +static void l_local_position(struct listener *l); +static void l_global_position_setpoint(struct listener *l); +static void l_local_position_setpoint(struct listener *l); +static void l_attitude_setpoint(struct listener *l); +static void l_actuator_outputs(struct listener *l); +static void l_actuator_armed(struct listener *l); +static void l_manual_control_setpoint(struct listener *l); +static void l_vehicle_attitude_controls(struct listener *l); +static void l_debug_key_value(struct listener *l); + +struct listener listeners[] = { + {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, + {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, + {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, + {l_vehicle_status, &status_sub, 0}, + {l_rc_channels, &rc_sub, 0}, + {l_global_position, &mavlink_subs.global_pos_sub, 0}, + {l_local_position, &mavlink_subs.local_pos_sub, 0}, + {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, + {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, + {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, + {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, + {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, + {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, +}; + +static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); + +void +l_sensor_combined(struct listener *l) +{ + struct sensor_combined_s raw; + + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); + + last_sensor_timestamp = raw.timestamp; + + /* send raw imu data */ + // mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, raw.accelerometer_raw[0], + // raw.accelerometer_raw[1], raw.accelerometer_raw[2], raw.gyro_raw[0], + // raw.gyro_raw[1], raw.gyro_raw[2], raw.magnetometer_raw[0], + // raw.magnetometer_raw[1], raw.magnetometer_raw[2]); + + /* mark individual fields as changed */ + uint16_t fields_updated = 0; + static unsigned accel_counter = 0; + static unsigned gyro_counter = 0; + static unsigned mag_counter = 0; + static unsigned baro_counter = 0; + + if (accel_counter != raw.accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = raw.accelerometer_counter; + } + + if (gyro_counter != raw.gyro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = raw.gyro_counter; + } + + if (mag_counter != raw.magnetometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = raw.magnetometer_counter; + } + + if (baro_counter != raw.baro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = raw.baro_counter; + } + + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, + raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], + raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], + raw.gyro_rad_s[1], raw.gyro_rad_s[2], + raw.magnetometer_ga[0], + raw.magnetometer_ga[1],raw.magnetometer_ga[2], + raw.baro_pres_mbar, 0 /* no diff pressure yet */, + raw.baro_alt_meter, raw.baro_temp_celcius, + fields_updated); + /* send pressure */ + //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, raw.timestamp / 1000, raw.baro_pres_mbar, raw.baro_alt_meter, raw.baro_temp_celcius * 100); + + sensors_raw_counter++; +} + +void +l_vehicle_attitude(struct listener *l) +{ + struct vehicle_attitude_s att; + + + /* copy attitude data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); + + /* send sensor values */ + mavlink_msg_attitude_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.roll, + att.pitch, + att.yaw, + att.rollspeed, + att.pitchspeed, + att.yawspeed); + + attitude_counter++; +} + +void +l_vehicle_gps_position(struct listener *l) +{ + struct vehicle_gps_position_s gps; + + /* copy gps data into local buffer */ + orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + + /* GPS position */ + mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, + gps.timestamp, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + gps.eph, + gps.epv, + gps.vel, + gps.cog, + gps.satellites_visible); + + if (gps.satellite_info_available && (gps_counter % 4 == 0)) { + mavlink_msg_gps_status_send(MAVLINK_COMM_0, + gps.satellites_visible, + gps.satellite_prn, + gps.satellite_used, + gps.satellite_elevation, + gps.satellite_azimuth, + gps.satellite_snr); + } + + gps_counter++; +} + +void +l_vehicle_status(struct listener *l) +{ + /* immediately communicate state changes back to user */ + orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + + /* enable or disable HIL */ + set_hil_on_off(v_status.flag_hil_enabled); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_mode, + v_status.state_machine, + mavlink_state); +} + +void +l_rc_channels(struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(chan, + rc.timestamp / 1000, + 0, + rc.chan[0].raw, + rc.chan[1].raw, + rc.chan[2].raw, + rc.chan[3].raw, + rc.chan[4].raw, + rc.chan[5].raw, + rc.chan[6].raw, + rc.chan[7].raw, + rc.rssi); +} + +void +l_global_position(struct listener *l) +{ + /* copy global position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); + + uint64_t timestamp = global_pos.timestamp; + int32_t lat = global_pos.lat; + int32_t lon = global_pos.lon; + int32_t alt = (int32_t)(global_pos.alt*1000); + int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); + int16_t vx = (int16_t)(global_pos.vx * 100.0f); + int16_t vy = (int16_t)(global_pos.vy * 100.0f); + int16_t vz = (int16_t)(global_pos.vz * 100.0f); + + /* heading in degrees * 10, from 0 to 36.000) */ + uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + + mavlink_msg_global_position_int_send(MAVLINK_COMM_0, + timestamp / 1000, + lat, + lon, + alt, + relative_alt, + vx, + vy, + vz, + hdg); +} + +void +l_local_position(struct listener *l) +{ + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); + + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, + local_pos.timestamp / 1000, + local_pos.x, + local_pos.y, + local_pos.z, + local_pos.vx, + local_pos.vy, + local_pos.vz); +} + +void +l_global_position_setpoint(struct listener *l) +{ + struct vehicle_global_position_setpoint_s global_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); + + uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + if (global_sp.altitude_is_relative) + coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + + mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + coordinate_frame, + global_sp.lat, + global_sp.lon, + global_sp.altitude, + global_sp.yaw); +} + +void +l_local_position_setpoint(struct listener *l) +{ + struct vehicle_local_position_setpoint_s local_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); + + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, + MAV_FRAME_LOCAL_NED, + local_sp.x, + local_sp.y, + local_sp.z, + local_sp.yaw); +} + +void +l_attitude_setpoint(struct listener *l) +{ + struct vehicle_attitude_setpoint_s att_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); + + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, + att_sp.timestamp/1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); +} + +void +l_actuator_outputs(struct listener *l) +{ + struct actuator_outputs_s act_outputs; + + orb_id_t ids[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + /* copy actuator data into local buffer */ + orb_copy(ids[l->arg], *l->subp, &act_outputs); + + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, + l->arg /* port number */, + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7]); +} + +void +l_actuator_armed(struct listener *l) +{ + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); +} + +void +l_manual_control_setpoint(struct listener *l) +{ + struct manual_control_setpoint_s man_control; + + /* copy manual control data into local buffer */ + orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); + + mavlink_msg_manual_control_send(MAVLINK_COMM_0, + mavlink_system.sysid, + man_control.roll * 1000, + man_control.pitch * 1000, + man_control.yaw * 1000, + man_control.throttle * 1000, + 0); +} + +void +l_vehicle_attitude_controls(struct listener *l) +{ + struct actuator_controls_s actuators; + + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators.control[3]); + + /* Only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + actuators.control[0], + actuators.control[1], + actuators.control[2], + actuators.control[3], + 0, + 0, + 0, + 0, + mavlink_mode, + 0); + } +} + +void +l_debug_key_value(struct listener *l) +{ + struct debug_key_value_s debug; + + orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); + + /* Enforce null termination */ + debug.key[sizeof(debug.key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + debug.key, + debug.value); +} + +static void * +uorb_receive_thread(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + const int timeout = 1000; + + /* + * Initialise listener array. + * + * Might want to invoke each listener once to set initial state. + */ + struct pollfd fds[n_listeners]; + for (unsigned i = 0; i < n_listeners; i++) { + fds[i].fd = *listeners[i].subp; + + /* Invoke callback to set initial state */ + //listeners[i].callback(&listener[i]); + } + + while (!thread_should_exit) { + + int poll_ret = poll(fds, n_listeners, timeout); + + /* handle the poll result */ + if (poll_ret == 0) { + mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + } else if (poll_ret < 0) { + mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + } else { + + for (unsigned i = 0; i < n_listeners; i++) { + if (fds[i].revents & POLLIN) + listeners[i].callback(&listeners[i]); + } + } + } + + return NULL; +} + +pthread_t +uorb_receive_start(void) +{ + /* --- SENSORS RAW VALUE --- */ + mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.sensor_sub, 100); /* 10Hz updates */ + + /* --- ATTITUDE VALUE --- */ + mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.att_sub, 100); /* 10Hz updates */ + + /* --- GPS VALUE --- */ + mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + + /* --- SYSTEM STATE --- */ + status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + + /* --- RC CHANNELS VALUE --- */ + rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(rc_sub, 100); /* 10Hz updates */ + + /* --- GLOBAL POS VALUE --- */ + mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ + + /* --- LOCAL POS VALUE --- */ + mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ + + /* --- GLOBAL SETPOINT VALUE --- */ + mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ + + /* --- LOCAL SETPOINT VALUE --- */ + mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ + + /* --- ATTITUDE SETPOINT VALUE --- */ + mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + + /* --- ACTUATOR OUTPUTS --- */ + mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR ARMED VALUE --- */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + + /* --- MAPPED MANUAL CONTROL INPUTS --- */ + mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR CONTROL VALUE --- */ + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ + + /* --- DEBUG VALUE OUTPUT --- */ + mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ + + /* start the listener loop */ + pthread_attr_t uorb_attr; + pthread_attr_init(&uorb_attr); + + /* Set stack size, needs more than 8000 bytes */ + /* XXX verify, should not need anything like this much unless MAVLink really sucks */ + pthread_attr_setstacksize(&uorb_attr, 8192); + + pthread_t thread; + pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + return thread; +} diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h new file mode 100644 index 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/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. From cf6e763c58b3934b93068c029add5842c47c52c5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Oct 2012 22:40:18 -0700 Subject: [PATCH 06/40] Beat up on the mavlink app startup a bit. --- apps/mavlink/mavlink.c | 151 ++++++++++++++--------------------- apps/systemlib/getopt_long.h | 4 +- 2 files changed, 62 insertions(+), 93 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 3a1365c662..3bb70973d6 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -61,6 +61,8 @@ #include #include +#include +#include #include "waypoints.h" #include "mavlink_log.h" @@ -118,7 +120,7 @@ static char mavlink_message_string[51] = {0}; /* protocol interface */ static int uart; -static int baudrate = 57600; +static int baudrate; /* interface mode */ static enum { @@ -128,12 +130,7 @@ static enum { static void mavlink_update_system(void); static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - -/** - * Print the usage - */ -static void usage(const char *reason); +static void usage(void); @@ -493,64 +490,60 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { - /* print welcome text */ - printf("[mavlink] MAVLink v1.0 serial interface starting..\n"); + static GETOPT_LONG_OPTION_T options[] = { + {"baud", REQUIRED_ARG, NULL, 'b'}, + {"speed", REQUIRED_ARG, NULL, 'b'}, + {"device", REQUIRED_ARG, NULL, 'd'}, + {"exit-allowed", NO_ARG, NULL, 'e'}, + {"onboard", NO_ARG, NULL, 'o'} + }; - /* create 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, longind; + char *device_name = "/dev/ttyS1"; baudrate = 57600; - /* read program arguments */ - int i; + while ((ch = getopt_long(argc, argv, "b:d:eo", options, &longind)) != EOF) { + switch (ch) { + case 'b': + baudrate = strtoul(optarg, NULL, 10); + if (baudrate == 0) + errx(1, "invalid baud rate '%s'", optarg); + break; - for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */ + case 'd': + device_name = 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 '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(); @@ -563,8 +556,6 @@ int mavlink_thread_main(int argc, char *argv[]) /* initialize waypoint manager */ mavlink_wpm_init(wpm); - uint16_t counter = 0; - /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 230400) { /* 200 Hz / 5 ms */ @@ -616,7 +607,7 @@ 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) { @@ -680,7 +671,6 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_missionlib_send_gcs_string(mavlink_message_string); mavlink_message_string[0] = '\0'; } - counter++; /* sleep 15 ms */ usleep(15000); @@ -691,54 +681,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 [--device ] [--speed ] [--exit-allowed] [--onboard]\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", @@ -746,25 +716,24 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + NULL); 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/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 From ae296175435f28b5480902fdf60f37e0c6de95ff Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Oct 2012 23:09:48 -0700 Subject: [PATCH 07/40] No joy with getopt_long --- apps/mavlink/mavlink.c | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 3bb70973d6..e893ea951c 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -62,7 +62,6 @@ #include #include #include -#include #include "waypoints.h" #include "mavlink_log.h" @@ -490,19 +489,15 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { - static GETOPT_LONG_OPTION_T options[] = { - {"baud", REQUIRED_ARG, NULL, 'b'}, - {"speed", REQUIRED_ARG, NULL, 'b'}, - {"device", REQUIRED_ARG, NULL, 'd'}, - {"exit-allowed", NO_ARG, NULL, 'e'}, - {"onboard", NO_ARG, NULL, 'o'} - }; - - int ch, longind; + int ch; char *device_name = "/dev/ttyS1"; baudrate = 57600; - while ((ch = getopt_long(argc, argv, "b:d:eo", options, &longind)) != EOF) { + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { switch (ch) { case 'b': baudrate = strtoul(optarg, NULL, 10); @@ -692,7 +687,7 @@ int mavlink_thread_main(int argc, char *argv[]) static void usage() { - fprintf(stderr, "usage: mavlink start [--device ] [--speed ] [--exit-allowed] [--onboard]\n" + fprintf(stderr, "usage: mavlink start [-d ] [-b ] [-e] [-o]\n" " mavlink stop\n" " mavlink status\n"); exit(1); @@ -716,7 +711,7 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, - NULL); + argv); exit(0); } From 642f3426a7aadd9fd345590a4c0881d7e64014a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Oct 2012 08:52:17 +0200 Subject: [PATCH 08/40] Added mag calibration routine, fixed minor typos without runtime effects --- apps/drivers/drv_mag.h | 5 +- apps/drivers/hmc5883/Makefile | 2 +- apps/drivers/hmc5883/hmc5883.cpp | 394 ++++++++++++++++++++++++++++++- apps/drivers/l3gd20/l3gd20.cpp | 2 +- 4 files changed, 394 insertions(+), 9 deletions(-) diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 1f5bb998e7..88381aaaa5 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -99,7 +99,10 @@ ORB_DECLARE(sensor_mag); /** copy the mag scaling constants to the structure pointed to by (arg) */ #define MAGIOCGSCALE _MAGIOC(3) +/** set the measurement range to handle (at least) arg Gauss */ +#define MAGIOCSRANGE _MAGIOC(4) + /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(4) +#define MAGIOCCALIBRATE _MAGIOC(5) #endif /* _DRV_MAG_H */ diff --git a/apps/drivers/hmc5883/Makefile b/apps/drivers/hmc5883/Makefile index 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..8931194073 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -175,6 +175,24 @@ private: */ void stop(); + /** + * Perform the on-sensor scale calibration routine. + * + * @note The sensor will continue to provide measurements, these + * will however reflect the uncalibrated sensor state until + * the calibration routine has been completed. + * + * @param enable set to 1 to enable self-test strap, 0 to disable + */ + int calibrate(unsigned enable); + + /** + * Set the sensor range. + * + * Sets the internal range to handle at least the argument in Gauss. + */ + int set_range(unsigned range); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -255,8 +273,8 @@ HMC5883::HMC5883(int bus) : _oldest_report(0), _reports(nullptr), _mag_topic(-1), - _range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */ - _range_ga(0.88f), + _range_scale(0), /* default range scale from counts to gauss */ + _range_ga(1.3f), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")) @@ -308,11 +326,71 @@ HMC5883::init() if (_mag_topic < 0) debug("failed to create sensor_mag object"); + /* set range */ + set_range(_range_ga); + ret = OK; out: return ret; } +int HMC5883::set_range(unsigned range) +{ + uint8_t range_bits; + + if (range < 1) { + range_bits = 0x00; + _range_scale = 1.0f / 1370.0f; + _range_ga = 0.88f; + } else if (range <= 1) { + range_bits = 0x01; + _range_scale = 1.0f / 1090.0f; + _range_ga = 1.3f; + } else if (range <= 2) { + range_bits = 0x02; + _range_scale = 1.0f / 820.0f; + _range_ga = 1.9f; + } else if (range <= 3) { + range_bits = 0x03; + _range_scale = 1.0f / 660.0f; + _range_ga = 2.5f; + } else if (range <= 4) { + range_bits = 0x04; + _range_scale = 1.0f / 440.0f; + _range_ga = 4.0f; + } else if (range <= 4.7f) { + range_bits = 0x05; + _range_scale = 1.0f / 390.0f; + _range_ga = 4.7f; + } else if (range <= 5.6f) { + range_bits = 0x06; + _range_scale = 1.0f / 330.0f; + _range_ga = 5.6f; + } else { + range_bits = 0x07; + _range_scale = 1.0f / 230.0f; + _range_ga = 8.1f; + } + + int ret; + + /* + * Send the command to set the range + */ + ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + + if (OK != ret) + perf_count(_comms_errors); + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + + if (OK != ret) + perf_count(_comms_errors); + + return !(range_bits_in == (range_bits << 5)); +} + int HMC5883::probe() { @@ -495,6 +573,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) /* not supported, always 1 sample per poll */ return -EINVAL; + case MAGIOCSRANGE: + return set_range(arg); + case MAGIOCSLOWPASS: /* not supported, no internal filtering */ return -EINVAL; @@ -510,8 +591,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 0; case MAGIOCCALIBRATE: - /* XXX perform auto-calibration */ - return -EINVAL; + return calibrate(arg); default: /* give it to the superclass */ @@ -718,6 +798,29 @@ out: return ret; } +int HMC5883::calibrate(unsigned enable) +{ + int ret; + /* arm the excitement strap */ + uint8_t conf_reg; + ret = read_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) + perf_count(_comms_errors); + if (enable) { + conf_reg |= 0x01; + } else { + conf_reg &= ~0x03; + } + ret = write_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) + perf_count(_comms_errors); + + uint8_t conf_reg_ret; + read_reg(ADDR_CONF_A, conf_reg_ret); + + return !(conf_reg == conf_reg_ret); +} + int HMC5883::write_reg(uint8_t reg, uint8_t val) { @@ -775,6 +878,7 @@ void start(); void test(); void reset(); void info(); +int calibrate(); /** * Start the driver. @@ -872,6 +976,273 @@ test() errx(0, "PASS"); } + +/** + * Automatic scale calibration. + * + * Basic idea: + * + * output = (ext field +- 1.1 Ga self-test) * scale factor + * + * and consequently: + * + * 1.1 Ga = (excited - normal) * scale factor + * scale factor = (excited - normal) / 1.1 Ga + * + * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 + * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 + * + * By subtracting the non-excited measurement the pure 1.1 Ga reading + * can be extracted and the sensitivity of all axes can be matched. + * + * SELF TEST OPERATION + * To check the HMC5883L for proper operation, a self test feature in incorporated + * in which the sensor offset straps are excited to create a nominal field strength + * (bias field) to be measured. To implement self test, the least significant bits + * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias) + * or 10 (negetive bias), e.g. 0x11 or 0x12. + * Then, by placing the mode register into single-measurement mode (0x01), + * two data acquisition cycles will be made on each magnetic vector. + * The first acquisition will be a set pulse followed shortly by measurement + * data of the external field. The second acquisition will have the offset strap + * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create + * about a ±1.1 gauss self test field plus the external field. The first acquisition + * values will be subtracted from the second acquisition, and the net measurement + * will be placed into the data output registers. + * Since self test adds ~1.1 Gauss additional field to the existing field strength, + * using a reduced gain setting prevents sensor from being saturated and data registers + * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3), + * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data + * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data + * output register. To leave the self test mode, change MS1 and MS0 bit of the + * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. + * Using the self test method described above, the user can scale sensor + */ +int calibrate() +{ + + struct mag_report report; + ssize_t sz; + int ret; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); + + /* start the sensor polling at 10 Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) + errx(1, "failed to set 2Hz poll rate"); + + /* Set to 2.5 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { + warnx("failed to set 2.5 Ga range"); + } + + if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { + warnx("failed to enable sensor calibration mode"); + } + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set null scale / offsets for mag"); + } + + float avg_excited[3]; + unsigned i; + + /* read the sensor 10x and report each value */ + for (i = 0; i < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } else { + avg_excited[0] += report.x; + avg_excited[1] += report.y; + avg_excited[2] += report.z; + } + + warnx("periodic read %u", i); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + } + + // warnx("starting calibration"); + + // struct mag_report report; + // ssize_t sz; + // int ret; + + // int fd = open(MAG_DEVICE_PATH, O_RDONLY); + // if (fd < 0) + // err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + // /* do a simple demand read */ + // sz = read(fd, &report, sizeof(report)); + // if (sz != sizeof(report)) + // err(1, "immediate read failed"); + + // warnx("single read"); + // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + // warnx("time: %lld", report.timestamp); + + // /* get scaling, set to zero */ + // struct mag_scale mscale_previous; + + // if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { + // warn("WARNING: failed to get scale / offsets for mag"); + // } + + // struct mag_scale mscale_null = { + // 0.0f, + // 1.0f, + // 0.0f, + // 1.0f, + // 0.0f, + // 1.0f, + // }; + + // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + // warn("WARNING: failed to set null scale / offsets for mag"); + // } + + // warnx("sensor ready"); + + // float avg_excited[3] = {0.0f, 0.0f, 0.0f}; + + // if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { + // warnx("failed to enable sensor calibration mode"); + // } + + // /* Set to 2.5 Gauss */ + // if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { + // warnx("failed to set 2.5 Ga range"); + // } + + // /* set the queue depth to 10 */ + // if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + // warnx("failed to set queue depth"); + // return 1; + // } else { + // warnx("set queue depth"); + // } + + // /* start the sensor polling at 100Hz */ + // if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 100)) { + // warnx("failed to set 100 Hz poll rate"); + // return 1; + // } else { + // warnx("set 100 Hz poll rate"); + // } + + // int i; + // for (i = 0; i < 10; i++) { + // struct pollfd fds; + + // (void) ioctl(fd, MAGIOCCALIBRATE, 1); + + // /* wait for data to be ready */ + // fds.fd = fd; + // fds.events = POLLIN; + // ret = poll(&fds, 1, 2000); + + // if (ret != 1) { + // warnx("timed out waiting for sensor data"); + // return 1; + // } + + // /* now go get it */ + // sz = read(fd, &report, sizeof(report)); + + // if (sz != sizeof(report)) { + // warn("periodic read failed"); + // return 1; + // } else { + // avg_excited[0] += report.x; + // avg_excited[1] += report.y; + // avg_excited[2] += report.z; + // } + // warnx("excited read %u", i); + // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + // warnx("time: %lld", report.timestamp); + + // } + + avg_excited[0] /= i; + avg_excited[1] /= i; + avg_excited[2] /= i; + + warnx("periodic excited reads %u", i); + warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); + + /* Set to 1.1 Gauss and end calibration */ + ret = ioctl(fd, MAGIOCCALIBRATE, 0); + ret = ioctl(fd, MAGIOCSRANGE, 1); + + float scaling[3]; + + /* calculate axis scaling */ + scaling[0] = 1.16f / avg_excited[0]; + /* second axis inverted */ + scaling[1] = 1.16f / -avg_excited[1]; + scaling[2] = 1.08f / avg_excited[2]; + + warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); + + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + } + + if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) { + warnx("failed to disable sensor calibration mode"); + } + + /* set scaling in device */ + // mscale_previous.x_scale = scaling[0]; + // mscale_previous.y_scale = scaling[1]; + // mscale_previous.z_scale = scaling[2]; + + // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + // warn("WARNING: failed to set new scale / offsets for mag"); + // } + + errx(0, "PASS"); +} + /** * Reset the driver. */ @@ -930,8 +1301,19 @@ hmc5883_main(int argc, char *argv[]) /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) hmc5883::info(); - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); + /* + * Autocalibrate the scaling + */ + if (!strcmp(argv[1], "calibrate")) { + if (hmc5883::calibrate() == 0) { + errx(0, "calibration successful"); + } else { + errx(1, "calibration failed"); + } + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); } diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 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'"); } From 084cde72f796817bdb02be7ec8cbf085d005e49b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Oct 2012 08:52:41 +0200 Subject: [PATCH 09/40] Reworked calibration --- apps/commander/commander.c | 362 ++++++++++++++++++++----------------- 1 file changed, 196 insertions(+), 166 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c569a6aa4a..086962abf2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -114,8 +114,8 @@ static bool commander_initialized = false; static struct vehicle_status_s current_status; /**< Main state machine */ static orb_advert_t stat_pub; -static uint16_t nofix_counter = 0; -static uint16_t gotfix_counter = 0; +// static uint16_t nofix_counter = 0; +// static uint16_t gotfix_counter = 0; static unsigned int failsafe_lowlevel_timeout_ms; @@ -124,7 +124,7 @@ 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 *command_handling_loop(void *arg); static void *orb_receive_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -164,7 +164,7 @@ static void usage(const char *reason); * @param a The array to sort * @param n The number of entries in the array */ -static void cal_bsort(float a[], int n); +// static void cal_bsort(float a[], int n); static int buzzer_init() { @@ -293,10 +293,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) struct sensor_combined_s raw; /* 30 seconds */ - int calibration_interval_ms = 30 * 1000; + uint64_t calibration_interval = 30 * 1000 * 1000; unsigned int calibration_counter = 0; - float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN}; float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); @@ -308,18 +308,45 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 0.0f, 1.0f, }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag"); + } + close(fd); - mavlink_log_info(mavlink_fd, "[commander] Please rotate around X"); - uint64_t calibration_start = hrt_absolute_time(); - while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) { + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = 0; + + while (hrt_absolute_time() < calibration_deadline) { /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + /* user guidance */ + if (hrt_absolute_time() > axis_deadline && + axis_index < 3) { + char buf[50]; + sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + + axis_deadline += calibration_interval / 3; + axis_index++; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[commander] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + if (poll(fds, 1, 1000)) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ @@ -353,8 +380,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } } - mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); - /* disable calibration mode */ status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); @@ -377,43 +402,49 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - 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]); + if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) { - /* announce and set new offset */ + /* 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_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"); + } + + char buf[50]; + sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000)); + mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + } else { + mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); } - 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); } @@ -467,45 +498,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); } @@ -560,61 +597,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); } @@ -732,8 +779,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -750,8 +799,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -813,37 +864,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 */ @@ -992,7 +1012,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 +1054,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 +1098,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 +1132,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; @@ -1470,7 +1499,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 +1509,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); From e8c4506a123145a9945bb1b1612cd031ec73c950 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Oct 2012 08:59:15 +0200 Subject: [PATCH 10/40] Minor documentation style fixes --- apps/mavlink/mavlink_parameters.c | 6 ++++-- apps/mavlink/mavlink_parameters.h | 4 +++- 2 files changed, 7 insertions(+), 3 deletions(-) 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 From 40abed787cde259218671db87d66b608535e0abd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Oct 2012 23:58:16 +0200 Subject: [PATCH 11/40] fixed wrong status indication of sd log command if startup fails --- apps/sdlog/sdlog.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 22b8d82ee6..229e1bfef6 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -131,7 +131,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); } @@ -202,7 +201,7 @@ int sdlog_thread_main(int argc, char *argv[]) { char folder_path[64]; if (create_logfolder(folder_path)) - errx(1, "unable to create logging folder, exiting"); + errx(1, "unable to create logging folder, exiting."); /* create sensorfile */ int sensorfile = -1; From 965bd35e2b925ad896567e77c3ac05e2ffa7af1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Oct 2012 00:02:53 +0200 Subject: [PATCH 12/40] Ignore measurement parts not projected to the plane --- apps/commander/commander.c | 38 +++++++++++++++++++++----------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 086962abf2..ef81605cd4 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -124,7 +124,6 @@ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ /* pthread loops */ -// static void *command_handling_loop(void *arg); static void *orb_receive_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -339,6 +338,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; } + if (!(axis_index < 3)) { + continue; + } + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); // if ((axis_left / 1000) == 0 && axis_left > 0) { @@ -351,26 +354,27 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - if (raw.magnetometer_ga[0] < mag_min[0]) { - mag_min[0] = raw.magnetometer_ga[0]; + /* ignore other axes */ + if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) { + mag_min[axis_index] = raw.magnetometer_ga[axis_index]; } - else if (raw.magnetometer_ga[0] > mag_max[0]) { - mag_max[0] = raw.magnetometer_ga[0]; + else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) { + mag_max[axis_index] = raw.magnetometer_ga[axis_index]; } - if (raw.magnetometer_ga[1] < mag_min[1]) { - mag_min[1] = raw.magnetometer_ga[1]; - } - else if (raw.magnetometer_ga[1] > mag_max[1]) { - mag_max[1] = raw.magnetometer_ga[1]; - } + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } - if (raw.magnetometer_ga[2] < mag_min[2]) { - mag_min[2] = raw.magnetometer_ga[2]; - } - else if (raw.magnetometer_ga[2] > mag_max[2]) { - mag_max[2] = raw.magnetometer_ga[2]; - } + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } calibration_counter++; } else { From 3ccc6849ac11851589c04adbf834091a4d918a01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Oct 2012 00:12:48 +0200 Subject: [PATCH 13/40] Fixed stupid typo in GPS app --- apps/sdlog/sdlog.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 229e1bfef6..49210de3da 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -346,14 +346,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++; From fef4362e79fb05ddf8caba4bb4365bab13b39ce6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Oct 2012 11:10:09 +0200 Subject: [PATCH 14/40] Merged new EKF version --- .gitignore | 2 +- apps/attitude_estimator_ekf/.context | 0 apps/attitude_estimator_ekf/Makefile | 5 +- .../attitude_estimator_ekf_main.c | 386 ++++++++------- .../attitude_estimator_ekf_params.c | 12 +- .../attitude_estimator_ekf_params.h | 0 .../codegen/attitudeKalmanfilter.c | 467 ++++++++++-------- .../codegen/attitudeKalmanfilter.h | 4 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 4 +- .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 4 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- apps/attitude_estimator_ekf/codegen/cross.c | 2 +- apps/attitude_estimator_ekf/codegen/cross.h | 4 +- apps/attitude_estimator_ekf/codegen/diag.c | 28 +- apps/attitude_estimator_ekf/codegen/diag.h | 11 +- apps/attitude_estimator_ekf/codegen/eye.c | 6 +- apps/attitude_estimator_ekf/codegen/eye.h | 4 +- .../attitude_estimator_ekf/codegen/mrdivide.c | 218 ++++---- .../attitude_estimator_ekf/codegen/mrdivide.h | 4 +- apps/attitude_estimator_ekf/codegen/norm.c | 30 +- apps/attitude_estimator_ekf/codegen/norm.h | 4 +- apps/attitude_estimator_ekf/codegen/power.c | 84 ---- apps/attitude_estimator_ekf/codegen/rdivide.c | 38 ++ .../codegen/{power.h => rdivide.h} | 16 +- .../attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- .../codegen/rt_defines.h | 2 +- .../codegen/rt_nonfinite.c | 2 +- .../codegen/rt_nonfinite.h | 2 +- .../attitude_estimator_ekf/codegen/rtwtypes.h | 6 +- apps/multirotor_att_control/Makefile | 0 .../multirotor_att_control_main.c | 6 +- .../multirotor_attitude_control.c | 6 +- .../multirotor_attitude_control.h | 0 .../multirotor_rate_control.c | 24 +- .../multirotor_rate_control.h | 0 apps/uORB/topics/sensor_combined.h | 36 +- 41 files changed, 709 insertions(+), 722 deletions(-) delete mode 100644 apps/attitude_estimator_ekf/.context mode change 100644 => 100755 apps/attitude_estimator_ekf/Makefile mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h delete mode 100755 apps/attitude_estimator_ekf/codegen/power.c create mode 100755 apps/attitude_estimator_ekf/codegen/rdivide.c rename apps/attitude_estimator_ekf/codegen/{power.h => rdivide.h} (52%) mode change 100644 => 100755 apps/multirotor_att_control/Makefile mode change 100644 => 100755 apps/multirotor_att_control/multirotor_att_control_main.c mode change 100644 => 100755 apps/multirotor_att_control/multirotor_attitude_control.c mode change 100644 => 100755 apps/multirotor_att_control/multirotor_attitude_control.h mode change 100644 => 100755 apps/multirotor_att_control/multirotor_rate_control.c mode change 100644 => 100755 apps/multirotor_att_control/multirotor_rate_control.h diff --git a/.gitignore b/.gitignore index 40957d3fb8..a3c279d2dd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,5 @@ .built -.context +*.context .depend .config .version 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 old mode 100644 new mode 100755 index e5620138a5..d6ec98f0bf --- 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,8 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rtGetNaN.c \ codegen/norm.c \ codegen/diag.c \ - codegen/cross.c \ - codegen/power.c + codegen/cross.c + # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c old mode 100644 new mode 100755 index 1d4df87fe3..b507b4c108 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -77,40 +77,40 @@ static float z_k[9]; /**< Measurement vector */ static float x_aposteriori_k[12]; /**< */ static float x_aposteriori[12]; 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, +}; 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 */ + 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 */ /* 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 +159,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); } @@ -250,12 +250,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 +271,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 +288,170 @@ 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]; + + 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 { + + /* 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}; + + /* 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; + } + + z_k[0] = raw.gyro_rad_s[0]; + z_k[1] = raw.gyro_rad_s[1]; + z_k[2] = raw.gyro_rad_s[2]; + + /* 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; + } + + 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]; + + 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); } - - z_k[0] = raw.gyro_rad_s[0]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[2]; - - /* 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; - } - - 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 old mode 100644 new mode 100755 index d95c56368b..f20d1b204b --- 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, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-1f); /* 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/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h old mode 100644 new mode 100755 diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 86d872fd2c..26a696af2c 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,19 +3,19 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 17:51:09 2012 * */ /* Include files */ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" +#include "rdivide.h" #include "norm.h" #include "cross.h" #include "eye.h" #include "mrdivide.h" #include "diag.h" -#include "power.h" /* Type Definitions */ @@ -32,10 +32,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)atan2((real32_T)b_u0, (real32_T)b_u1); } else if (u1 == 0.0F) { if (u0 > 0.0F) { y = RT_PIF / 2.0F; @@ -45,7 +59,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) y = 0.0F; } } else { - y = (real32_T)atan2f(u0, u1); + y = (real32_T)atan2(u0, u1); } return y; @@ -60,30 +74,41 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { - real32_T a[12]; - int32_T i; - real32_T b_a[12]; - real32_T Q[144]; real32_T O[9]; real_T dv0[9]; - real32_T c_a[9]; - real32_T d_a[9]; + real32_T a[9]; + int32_T i; + real32_T b_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; + static const real32_T fv0[144] = { 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.1F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.1F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.1F }; + 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, + 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, 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 }; @@ -91,46 +116,45 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const 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, 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 fv1[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 }; + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - real32_T fv1[36]; + real32_T fv2[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 }; + 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, + 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, 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, 1, 0, 0, 0, 0, 0, 0, 1, 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, 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, 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, + 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, 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 fv3[6]; real32_T b_z[6]; - real32_T b_y; /* Extended Attitude Kalmanfilter */ /* */ @@ -146,32 +170,37 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* coder.varsize('udpIndVect', [9,1], [1,0]) */ /* udpIndVect=find(updVect); */ /* process and measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */ - power(q, 2.0, a); - for (i = 0; i < 12; i++) { - b_a[i] = a[i] * dt; - } - - diag(b_a, Q); - + /* Q = diag(q.^2*dt); */ + /* 'attitudeKalmanfilter:29' Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:30' 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:31' 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:32' 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ + /* 'attitudeKalmanfilter:33' 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ + /* 'attitudeKalmanfilter:34' 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:35' 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:36' 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:37' 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */ + /* 'attitudeKalmanfilter:38' 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */ + /* 'attitudeKalmanfilter:39' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */ + /* 'attitudeKalmanfilter:40' 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 +211,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; + 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; + b_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 +248,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] += 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] += b_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 +342,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 +355,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 + fv0[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); */ - b_diag(r, R); + /* 'attitudeKalmanfilter:103' R=diag(r); */ + 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, Es, 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 +398,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]; + fv1[i + 9 * i0] = f0 + R[i + 9 * i0]; } } - mrdivide(b_P_apriori, fv0, K_k); + mrdivide(b_P_apriori, fv1, 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; + 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] * 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; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -413,22 +445,23 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 + * i0]; } } } } 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)); */ - c_diag(*(real32_T (*)[3])&r[0], O); + /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */ + b_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, Es, 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; @@ -441,9 +474,9 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { - fv1[i + 3 * i0] = 0.0F; + fv2[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * + fv2[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } @@ -451,46 +484,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 += fv2[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; } - c_a[i + 3 * i0] = y + O[i + 3 * i0]; + a[i + 3 * i0] = f0 + O[i + 3 * i0]; } } - b_mrdivide(c_P_apriori, c_a, S_k); + b_mrdivide(c_P_apriori, 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; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -498,25 +531,25 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } 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)); */ - d_diag(*(real32_T (*)[6])&r[0], S_k); + /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */ + c_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, Es, 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 +572,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]; + fv2[i + 6 * i0] = f0 + S_k[i + 6 * i0]; } } - c_mrdivide(d_P_apriori, fv1, b_K_k); + c_mrdivide(d_P_apriori, fv2, 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; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -586,22 +619,22 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } 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, Es, 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 +652,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 +674,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]; } @@ -651,33 +684,33 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } for (i = 0; i < 6; i++) { - fv2[i] = 0.0F; + fv3[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { - fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; + fv3[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } - b_z[i] = b_r[i] - fv2[i]; + b_z[i] = b_r[i] - fv3[i]; } 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; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -685,66 +718,74 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * + P_apriori[i1 + 12 * i0]; } } } } 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=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: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]); } -/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index f8f99fa5a3..9d89705a6f 100755 --- 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: Sat Oct 13 16:28:18 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 index 689bc49e94..27eb2763e5 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index dcce3cd728..277df273df 100755 --- 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: Sat Oct 13 16:28:18 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 index 39f8f648c8..ed09930394 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index ea7b9e03ea..0b0ccd0730 100755 --- 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: Sat Oct 13 16:28:18 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 index 6d5704a7a8..95814863fe 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c index 27da4b6b93..d4524fed8e 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h index 8ef2c475c2..d70b0adc24 100755 --- 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: Sat Oct 13 16:28:18 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 index 81626589d2..58e9f553f4 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ @@ -27,19 +27,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)); - for (j = 0; j < 9; j++) { - d[j + 9 * j] = v[j]; - } -} - -/* - * - */ -void c_diag(const real32_T v[3], real32_T d[9]) +void b_diag(const real32_T v[3], real32_T d[9]) { int32_T j; for (j = 0; j < 9; j++) { @@ -54,10 +42,10 @@ void c_diag(const real32_T v[3], real32_T d[9]) /* * */ -void d_diag(const real32_T v[6], real32_T d[36]) +void c_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]; } @@ -66,12 +54,12 @@ void d_diag(const real32_T v[6], real32_T d[36]) /* * */ -void diag(const real32_T v[12], real32_T d[144]) +void diag(const real32_T v[9], real32_T d[81]) { int32_T j; - memset((void *)&d[0], 0, 144U * sizeof(real32_T)); - for (j = 0; j < 12; j++) { - d[j + 12 * j] = v[j]; + memset(&d[0], 0, 81U * sizeof(real32_T)); + for (j = 0; j < 9; j++) { + d[j + 9 * j] = v[j]; } } diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h index 10cea81b24..85ff0e0e90 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __DIAG_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" @@ -29,9 +29,8 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_diag(const real32_T v[9], real32_T d[81]); -extern void c_diag(const real32_T v[3], real32_T d[9]); -extern void d_diag(const real32_T v[6], real32_T d[36]); -extern void diag(const real32_T v[12], real32_T d[144]); +extern void b_diag(const real32_T v[3], real32_T d[9]); +extern void c_diag(const real32_T v[6], real32_T d[36]); +extern void diag(const real32_T v[9], real32_T d[81]); #endif /* End of code generation (diag.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index 2db070e6fe..d7ae20afda 100755 --- 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: Sat Oct 13 16:28:18 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 index 027db1c065..77099ec22e 100755 --- 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: Sat Oct 13 16:28:18 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 index ce29e42cd0..1c69f1ef77 100755 --- 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: Sat Oct 13 16:28:18 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 index b80f34357f..75a3968783 100755 --- 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: Sat Oct 13 16:28:18 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 index 341c930220..fbd5d43e07 100755 --- 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: Sat Oct 13 16:28:18 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 index 0f58dbe694..a2e9d90d45 100755 --- 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: Sat Oct 13 16:28:18 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 deleted file mode 100755 index 8672c7a85c..0000000000 --- a/apps/attitude_estimator_ekf/codegen/power.c +++ /dev/null @@ -1,84 +0,0 @@ -/* - * power.c - * - * Code generation for function 'power' - * - * C source code generated on: Mon Oct 01 19:38:49 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "power.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T f0; - real32_T f1; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else { - f0 = (real32_T)fabsf(u0); - f1 = (real32_T)fabsf(u1); - if (rtIsInfF(u1)) { - if (f0 == 1.0F) { - y = ((real32_T)rtNaN); - } else if (f0 > 1.0F) { - if (u1 > 0.0F) { - y = ((real32_T)rtInf); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = ((real32_T)rtInf); - } - } else if (f1 == 0.0F) { - y = 1.0F; - } else if (f1 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } 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)rtNaN); - } else { - y = (real32_T)powf(u0, u1); - } - } - - return y; -} - -/* - * - */ -void power(const real32_T a[12], real_T b, real32_T y[12]) -{ - int32_T k; - for (k = 0; k < 12; k++) { - y[k] = rt_powf_snf(a[k], (real32_T)b); - } -} - -/* End of code generation (power.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c new file mode 100755 index 0000000000..32d75bf28d --- /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: Sat Oct 13 16:28:18 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/power.h b/apps/attitude_estimator_ekf/codegen/rdivide.h similarity index 52% rename from apps/attitude_estimator_ekf/codegen/power.h rename to apps/attitude_estimator_ekf/codegen/rdivide.h index a60f1bb25f..e488ed87f9 100755 --- a/apps/attitude_estimator_ekf/codegen/power.h +++ b/apps/attitude_estimator_ekf/codegen/rdivide.h @@ -1,17 +1,17 @@ /* - * power.h + * rdivide.h * - * Code generation for function 'power' + * Code generation for function 'rdivide' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ -#ifndef __POWER_H__ -#define __POWER_H__ +#ifndef __RDIVIDE_H__ +#define __RDIVIDE_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 rdivide(const real32_T x[3], real32_T y, real32_T z[3]); #endif -/* End of code generation (power.h) */ +/* End of code generation (rdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 53686acc98..5500ef3732 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index 5ac1dc7943..bfef3881ea 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 1e1876b80f..e5113aef0b 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 5f1c81f676..28f35e1f13 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h index 5f65f6de98..3ce17dd0fc 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 2c687d7a1f..6f58e7265e 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index d2ebd0806e..5c2a0aa619 100755 --- 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: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index b487c8b384..4fbb93f578 100755 --- 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: Sat Oct 13 16:28:18 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/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 old mode 100644 new mode 100755 index 7d5821d3ff..63c7d0097d --- 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); @@ -196,6 +196,7 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; + //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); } @@ -212,6 +213,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; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c old mode 100644 new mode 100755 index 2129915d12..458b86057b --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -238,12 +238,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff, - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT)*1/10.0f; /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT)*1/10.0f; /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f; /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h old mode 100644 new mode 100755 diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c old mode 100644 new mode 100755 index 7532dffa26..2809c4533c --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -50,16 +50,16 @@ #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, 20.0f); /* same on Flamewheel */ + PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); + PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); + PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.f); -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_P, 40.0f); /* 0.15 F405 Flamewheel */ +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, 10.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { @@ -175,14 +175,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ - float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last); + float pitch_control = p.attrate_p * deltaT *((rate_sp->pitch)*p.attrate_lim-rates[1])-p.attrate_d*(pitch_control_last); pitch_control_last=pitch_control; /* control roll (left/right) output */ - float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last); + float roll_control = p.attrate_p * deltaT * ((rate_sp->roll)*p.attrate_lim-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 * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2] ); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h old mode 100644 new mode 100755 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 */ }; From b50bc7798ac463de3e0c3147df46a3f7227df8c3 Mon Sep 17 00:00:00 2001 From: daregger Date: Tue, 16 Oct 2012 16:49:45 +0200 Subject: [PATCH 15/40] Wip on inner rate loop --- .gitignore | 1 + .../attitude_estimator_ekf_main.c | 9 ++-- apps/commander/state_machine_helper.c | 6 +-- .../multirotor_rate_control.c | 41 +++++++++++++------ 4 files changed, 37 insertions(+), 20 deletions(-) mode change 100755 => 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c mode change 100755 => 100644 apps/multirotor_att_control/multirotor_rate_control.c diff --git a/.gitignore b/.gitignore index 38cb13162b..ea416fae48 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,7 @@ apps/namedapp/namedapp_proto.h Make.dep *.o *.a +*~ Images/*.bin Images/*.px4 nuttx/Make.defs diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c old mode 100755 new mode 100644 index b507b4c108..a291a49146 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -293,12 +293,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; 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 { @@ -315,9 +316,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]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[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) { 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/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c old mode 100755 new mode 100644 index 2809c4533c..c132665d21 --- 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" @@ -50,10 +54,11 @@ #include #include - PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ - PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); - PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); - PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* 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, 40.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 40.0f); /* 0.15 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f); @@ -144,8 +149,8 @@ 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; last_run = hrt_absolute_time(); @@ -174,15 +179,25 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* 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); - pitch_control_last=pitch_control; - /* control roll (left/right) output */ + /* increase resilience to faulty control inputs */ + if (isfinite(pitch_control)) { + pitch_control_last = pitch_control; + } else { + pitch_control = 0.0f; + } + /* 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); - roll_control_last=roll_control; + /* increase resilience to faulty control inputs */ + if (isfinite(roll_control)) { + roll_control_last = roll_control; + } else { + roll_control = 0.0f; + } + /* 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)*p.attrate_lim-rates[2]); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; From 32e586d4b7561d1018e29aa59f572c3bac625024 Mon Sep 17 00:00:00 2001 From: daregger Date: Tue, 16 Oct 2012 18:02:28 +0200 Subject: [PATCH 16/40] Controller and estimator updates --- apps/attitude_estimator_ekf/Makefile | 1 + .../attitudeKalmanfilter.m | 0 .../attitudeKalmanfilter.prj | 0 .../attitude_estimator_ekf_params.c | 0 .../attitude_estimator_ekf_params.h | 0 .../codegen/attitudeKalmanfilter.c | 167 +++++++------- .../codegen/attitudeKalmanfilter.h | 2 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 2 +- .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 2 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- apps/attitude_estimator_ekf/codegen/cross.c | 2 +- apps/attitude_estimator_ekf/codegen/cross.h | 2 +- apps/attitude_estimator_ekf/codegen/diag.c | 26 ++- apps/attitude_estimator_ekf/codegen/diag.h | 9 +- apps/attitude_estimator_ekf/codegen/eye.c | 2 +- apps/attitude_estimator_ekf/codegen/eye.h | 2 +- .../attitude_estimator_ekf/codegen/mrdivide.c | 2 +- .../attitude_estimator_ekf/codegen/mrdivide.h | 2 +- apps/attitude_estimator_ekf/codegen/norm.c | 2 +- apps/attitude_estimator_ekf/codegen/norm.h | 2 +- apps/attitude_estimator_ekf/codegen/power.c | 84 +++++++ apps/attitude_estimator_ekf/codegen/power.h | 34 +++ apps/attitude_estimator_ekf/codegen/rdivide.c | 2 +- apps/attitude_estimator_ekf/codegen/rdivide.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- .../codegen/rt_defines.h | 2 +- .../codegen/rt_nonfinite.c | 2 +- .../codegen/rt_nonfinite.h | 2 +- .../attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- .../multirotor_att_control_main.c | 19 +- .../multirotor_attitude_control.c | 207 ++++++------------ .../multirotor_attitude_control.h | 0 .../multirotor_rate_control.h | 0 38 files changed, 327 insertions(+), 268 deletions(-) mode change 100755 => 100644 apps/attitude_estimator_ekf/Makefile mode change 100755 => 100644 apps/attitude_estimator_ekf/attitudeKalmanfilter.m mode change 100755 => 100644 apps/attitude_estimator_ekf/attitudeKalmanfilter.prj mode change 100755 => 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c mode change 100755 => 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/cross.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/cross.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/diag.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/diag.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/eye.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/eye.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/mrdivide.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/mrdivide.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/norm.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/norm.h create mode 100644 apps/attitude_estimator_ekf/codegen/power.c create mode 100644 apps/attitude_estimator_ekf/codegen/power.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rdivide.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rdivide.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rtGetInf.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rtGetInf.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rtGetNaN.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rtGetNaN.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rt_defines.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rt_nonfinite.c mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rt_nonfinite.h mode change 100755 => 100644 apps/attitude_estimator_ekf/codegen/rtwtypes.h mode change 100755 => 100644 apps/multirotor_att_control/multirotor_att_control_main.c mode change 100755 => 100644 apps/multirotor_att_control/multirotor_attitude_control.c mode change 100755 => 100644 apps/multirotor_att_control/multirotor_attitude_control.h mode change 100755 => 100644 apps/multirotor_att_control/multirotor_rate_control.h diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile old mode 100755 new mode 100644 index d6ec98f0bf..4d531867ce --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -48,6 +48,7 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rtGetNaN.c \ codegen/norm.c \ codegen/diag.c \ + codegen/power.c \ codegen/cross.c 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_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c old mode 100755 new mode 100644 diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h old mode 100755 new mode 100644 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 26a696af2c..885c01bf3d --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Sat Oct 13 17:51:09 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -16,6 +16,7 @@ #include "eye.h" #include "mrdivide.h" #include "diag.h" +#include "power.h" /* Type Definitions */ @@ -74,11 +75,14 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { + real32_T a[12]; + real32_T b_a[12]; + int32_T i; + real32_T Q[144]; real32_T O[9]; real_T dv0[9]; - real32_T a[9]; - int32_T i; - real32_T b_a[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]; @@ -91,69 +95,55 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const int32_T i1; real32_T P_apriori[144]; real32_T f0; - static const real32_T fv0[144] = { 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.1F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.1F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.1F }; - 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, 0, 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, 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 fv1[81]; + 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, 0, 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 fv2[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, + real32_T fv1[36]; + 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, 0, 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, 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, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 0, 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 fv3[6]; + real32_T fv2[6]; real32_T b_z[6]; /* Extended Attitude Kalmanfilter */ @@ -170,19 +160,26 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* coder.varsize('udpIndVect', [9,1], [1,0]) */ /* udpIndVect=find(updVect); */ /* process and measurement noise covariance matrix */ - /* Q = diag(q.^2*dt); */ - /* 'attitudeKalmanfilter:29' Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:30' 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:31' 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:32' 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ - /* 'attitudeKalmanfilter:33' 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ - /* 'attitudeKalmanfilter:34' 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:35' 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:36' 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:37' 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */ - /* 'attitudeKalmanfilter:38' 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */ - /* 'attitudeKalmanfilter:39' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */ - /* 'attitudeKalmanfilter:40' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */ + /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */ + 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:44' wx= x_aposteriori_k(1); */ /* 'attitudeKalmanfilter:45' wy= x_aposteriori_k(2); */ @@ -214,13 +211,13 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); for (i = 0; i < 9; i++) { - a[i] = (real32_T)dv0[i] + O[i] * dt; + c_a[i] = (real32_T)dv0[i] + O[i] * dt; } /* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); for (i = 0; i < 9; i++) { - b_a[i] = (real32_T)dv0[i] + O[i] * dt; + d_a[i] = (real32_T)dv0[i] + O[i] * dt; } /* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */ @@ -250,12 +247,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 3; i++) { m_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - m_n_b[i] += a[i + 3 * i0] * x_n_b[i0]; + m_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; } z_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - z_n_b[i] += b_a[i + 3 * i0] * b_x_aposteriori_k[i0]; + z_n_b[i] += d_a[i + 3 * i0] * b_x_aposteriori_k[i0]; } x_apriori[i + 6] = m_n_b[i]; @@ -360,7 +357,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } - P_apriori[i + 12 * i0] = f0 + fv0[i + 12 * i0]; + P_apriori[i + 12 * i0] = f0 + Q[i + 12 * i0]; } } @@ -368,10 +365,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { /* 'attitudeKalmanfilter:103' R=diag(r); */ - diag(r, R); + b_diag(r, R); /* observation matrix */ - /* 'attitudeKalmanfilter:106' H_k=[ E, Es, Z, Z; */ + /* '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; */ @@ -403,11 +400,11 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; } - fv1[i + 9 * i0] = f0 + R[i + 9 * i0]; + fv0[i + 9 * i0] = f0 + R[i + 9 * i0]; } } - mrdivide(b_P_apriori, fv1, K_k); + mrdivide(b_P_apriori, fv0, K_k); /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { @@ -416,13 +413,13 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; } - a[i] = z[i] - f0; + c_a[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * a[i0]; + f0 += K_k[i + 12 * i0] * c_a[i0]; } x_aposteriori[i] = x_apriori[i] + f0; @@ -437,7 +434,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; } - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -445,8 +442,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } @@ -455,10 +451,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */ - b_diag(*(real32_T (*)[3])&r[0], O); + c_diag(*(real32_T (*)[3])&r[0], O); /* observation matrix */ - /* 'attitudeKalmanfilter:123' H_k=[ E, Es, Z, Z]; */ + /* '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)); */ @@ -474,9 +470,9 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { - fv2[i + 3 * i0] = 0.0F; + fv1[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - fv2[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * + fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } @@ -486,14 +482,14 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 3; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += fv2[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; + f0 += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; } - a[i + 3 * i0] = f0 + O[i + 3 * i0]; + c_a[i + 3 * i0] = f0 + O[i + 3 * i0]; } } - b_mrdivide(c_P_apriori, a, S_k); + b_mrdivide(c_P_apriori, c_a, S_k); /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { @@ -523,7 +519,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; } - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -531,8 +527,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + - 12 * i0]; + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * + i0]; } } } @@ -542,10 +538,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */ - c_diag(*(real32_T (*)[6])&r[0], S_k); + d_diag(*(real32_T (*)[6])&r[0], S_k); /* observation matrix */ - /* 'attitudeKalmanfilter:138' H_k=[ E, Es, Z, Z; */ + /* '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); */ @@ -577,11 +573,11 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; } - fv2[i + 6 * i0] = f0 + S_k[i + 6 * i0]; + fv1[i + 6 * i0] = f0 + S_k[i + 6 * i0]; } } - c_mrdivide(d_P_apriori, fv2, b_K_k); + c_mrdivide(d_P_apriori, fv1, b_K_k); /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { @@ -611,7 +607,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; } - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -619,8 +615,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 - + 12 * i0]; + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * + i0]; } } } @@ -631,7 +627,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const { /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */ /* observation matrix */ - /* 'attitudeKalmanfilter:154' H_k=[ E, Es, Z, Z; */ + /* '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); */ @@ -684,12 +680,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } for (i = 0; i < 6; i++) { - fv3[i] = 0.0F; + fv2[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { - fv3[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; + fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } - b_z[i] = b_r[i] - fv3[i]; + b_z[i] = b_r[i] - fv2[i]; } for (i = 0; i < 12; i++) { @@ -710,7 +706,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -718,8 +714,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * - P_apriori[i1 + 12 * i0]; + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 + * i0]; } } } @@ -785,7 +781,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* 'attitudeKalmanfilter:195' psi=atan2(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[1] = -(real32_T)asin(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } +/* End of code generation (attitudeKalmanfilter.c) */ 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 9d89705a6f..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 27eb2763e5..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: Sat Oct 13 16:28:18 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 277df273df..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 ed09930394..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: Sat Oct 13 16:28:18 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 0b0ccd0730..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 95814863fe..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: Sat Oct 13 16:28:18 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 d4524fed8e..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: Sat Oct 13 16:28:18 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 d70b0adc24..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 58e9f553f4..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -27,7 +27,19 @@ /* * */ -void b_diag(const real32_T v[3], real32_T d[9]) +void b_diag(const real32_T v[9], real32_T d[81]) +{ + int32_T j; + memset(&d[0], 0, 81U * sizeof(real32_T)); + for (j = 0; j < 9; j++) { + d[j + 9 * j] = v[j]; + } +} + +/* + * + */ +void c_diag(const real32_T v[3], real32_T d[9]) { int32_T j; for (j = 0; j < 9; j++) { @@ -42,7 +54,7 @@ void b_diag(const real32_T v[3], real32_T d[9]) /* * */ -void c_diag(const real32_T v[6], real32_T d[36]) +void d_diag(const real32_T v[6], real32_T d[36]) { int32_T j; memset(&d[0], 0, 36U * sizeof(real32_T)); @@ -54,12 +66,12 @@ void c_diag(const real32_T v[6], real32_T d[36]) /* * */ -void diag(const real32_T v[9], real32_T d[81]) +void diag(const real32_T v[12], real32_T d[144]) { int32_T j; - memset(&d[0], 0, 81U * sizeof(real32_T)); - for (j = 0; j < 9; j++) { - d[j + 9 * j] = v[j]; + 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 85ff0e0e90..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ @@ -29,8 +29,9 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_diag(const real32_T v[3], real32_T d[9]); -extern void c_diag(const real32_T v[6], real32_T d[36]); -extern void diag(const real32_T v[9], real32_T d[81]); +extern void b_diag(const real32_T v[9], real32_T d[81]); +extern void c_diag(const real32_T v[3], real32_T d[9]); +extern void d_diag(const real32_T v[6], real32_T d[36]); +extern void diag(const real32_T v[12], real32_T d[144]); #endif /* End of code generation (diag.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 d7ae20afda..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 77099ec22e..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 1c69f1ef77..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 75a3968783..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 fbd5d43e07..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 a2e9d90d45..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c new file mode 100644 index 0000000000..fc602fb5ce --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/power.c @@ -0,0 +1,84 @@ +/* + * power.c + * + * Code generation for function 'power' + * + * C source code generated on: Tue Oct 16 15:27:58 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "power.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1); + +/* Function Definitions */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1) +{ + real32_T y; + real32_T f1; + real32_T f2; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else { + f1 = (real32_T)fabs(u0); + f2 = (real32_T)fabs(u1); + if (rtIsInfF(u1)) { + if (f1 == 1.0F) { + y = ((real32_T)rtNaN); + } else if (f1 > 1.0F) { + if (u1 > 0.0F) { + y = ((real32_T)rtInf); + } else { + y = 0.0F; + } + } else if (u1 > 0.0F) { + y = 0.0F; + } else { + y = ((real32_T)rtInf); + } + } else if (f2 == 0.0F) { + y = 1.0F; + } else if (f2 == 1.0F) { + if (u1 > 0.0F) { + y = u0; + } else { + y = 1.0F / u0; + } + } else if (u1 == 2.0F) { + y = u0 * u0; + } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { + y = (real32_T)sqrt(u0); + } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { + y = ((real32_T)rtNaN); + } else { + y = (real32_T)pow(u0, u1); + } + } + + return y; +} + +/* + * + */ +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], 2.0F); + } +} + +/* End of code generation (power.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/power.h b/apps/attitude_estimator_ekf/codegen/power.h new file mode 100644 index 0000000000..40a0d9353f --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/power.h @@ -0,0 +1,34 @@ +/* + * power.h + * + * Code generation for function 'power' + * + * C source code generated on: Tue Oct 16 15:27:58 2012 + * + */ + +#ifndef __POWER_H__ +#define __POWER_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 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 old mode 100755 new mode 100644 index 32d75bf28d..11e4e6f1f8 --- a/apps/attitude_estimator_ekf/codegen/rdivide.c +++ b/apps/attitude_estimator_ekf/codegen/rdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'rdivide' * - * C source code generated on: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h old mode 100755 new mode 100644 index e488ed87f9..4e24303748 --- a/apps/attitude_estimator_ekf/codegen/rdivide.h +++ b/apps/attitude_estimator_ekf/codegen/rdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'rdivide' * - * C source code generated on: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ 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 5500ef3732..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: Sat Oct 13 16:28:18 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 bfef3881ea..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: Sat Oct 13 16:28:18 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 e5113aef0b..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: Sat Oct 13 16:28:18 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 28f35e1f13..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: Sat Oct 13 16:28:18 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 3ce17dd0fc..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: Sat Oct 13 16:28:18 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 6f58e7265e..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: Sat Oct 13 16:28:18 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 5c2a0aa619..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: Sat Oct 13 16:28:18 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 4fbb93f578..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: Sat Oct 13 16:28:18 2012 + * C source code generated on: Tue Oct 16 15:27:58 2012 * */ diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c old mode 100755 new mode 100644 index 63c7d0097d..cb82036fe8 --- 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,6 +193,7 @@ 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; @@ -203,7 +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, remove, switch to yaw rate controller + att_sp.yaw_body = manual.yaw; // XXX Hack, clean up /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; @@ -226,21 +227,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 old mode 100755 new mode 100644 index 458b86057b..00b759d73a --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -54,17 +54,13 @@ #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_I, 0.0f); @@ -76,17 +72,17 @@ 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 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 yawrate_p; + // float yawrate_i; + // float yawrate_d; + // float yawrate_awu; + // float yawrate_lim; float att_p; float att_i; @@ -99,17 +95,17 @@ 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 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 yawrate_p; + // param_t yawrate_i; + // param_t yawrate_d; + // param_t yawrate_awu; + // param_t yawrate_lim; param_t att_p; param_t att_i; @@ -137,17 +133,17 @@ 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->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->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->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); @@ -163,17 +159,17 @@ 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->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->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->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); @@ -188,16 +184,23 @@ 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 yaw_speed_controller; static PID_t pitch_controller; static PID_t roll_controller; @@ -213,112 +216,44 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s // 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(&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); + PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + 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(&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)); } /* 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)*1/10.0f; + rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff, + att->pitch, att->pitchspeed, deltaT)*1/10.0f; /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, - att->roll, att->rollspeed, deltaT)*1/10.0f; + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, + att->roll, att->rollspeed, deltaT)*1/10.0f; /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f; - - /* - * 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; - } + //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)); - 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; - } + rates_sp->thrust = att_sp->thrust; - 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; - } motor_skip_counter++; } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h old mode 100755 new mode 100644 diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h old mode 100755 new mode 100644 From 97726fa67904650c8d82ec7da58924f261deb125 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 08:02:50 +0200 Subject: [PATCH 17/40] Calibration WIP, not compiling --- apps/commander/commander.c | 17 ++ apps/drivers/drv_mag.h | 3 + apps/drivers/hmc5883/hmc5883.cpp | 362 +++++++++++++------------------ apps/sensors/sensor_params.c | 4 + apps/sensors/sensors.cpp | 19 +- 5 files changed, 192 insertions(+), 213 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 43df6ceb28..f11c583d41 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -299,6 +299,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); + + + struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -396,6 +399,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * to shift the center to zero * * offset = max - ((max - min) / 2.0f) + * max - max/2 + min/2 + * max/2 + min/2 * * which reduces to * @@ -422,6 +427,18 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); } + if (param_set(param_find("SENS_MAG_XSCALE"), &(mag_offset[0]))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mag_offset[1]))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(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], diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 88381aaaa5..7ba9dd6956 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -105,4 +105,7 @@ ORB_DECLARE(sensor_mag); /** perform self-calibration, update scale factors to canonical units */ #define MAGIOCCALIBRATE _MAGIOC(5) +/** excite strap */ +#define MAGIOCEXSTRAP _MAGIOC(6) + #endif /* _DRV_MAG_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 8931194073..fc80c8d177 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -186,6 +186,18 @@ private: */ int calibrate(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. * @@ -593,6 +605,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCCALIBRATE: return calibrate(arg); + case MAGIOCEXSTRAP: + return set_excitement(arg); + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -799,6 +814,139 @@ out: } int HMC5883::calibrate(unsigned enable) +{ + struct mag_report report; + ssize_t sz; + int ret; + + int fd = (int)enable; + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("starting mag scale calibration"); + warnx("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(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); + + /* start the sensor polling at 50 Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50)) + errx(1, "failed to set 2Hz poll rate"); + + /* Set to 2.5 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { + warnx("failed to set 2.5 Ga range"); + } + + if (OK != ioctl(fd, MAGIOCPOSEX, 1)) { + warnx("failed to enable sensor calibration mode"); + } + + struct mag_scale mscale_previous = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to get scale / offsets for mag"); + } + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set null scale / offsets for mag"); + } + + float avg_excited[3]; + unsigned i; + + /* 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) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } else { + avg_excited[0] += report.x; + avg_excited[1] += report.y; + avg_excited[2] += report.z; + } + + // warnx("periodic read %u", i); + // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + // warnx("time: %lld", report.timestamp); + } + + avg_excited[0] /= i; + avg_excited[1] /= i; + avg_excited[2] /= i; + + warnx("periodic excited reads %u", i); + warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); + + /* Set to 1.1 Gauss and end calibration */ + ret = ioctl(fd, MAGIOCNONEX, 0); + ret = ioctl(fd, MAGIOCSRANGE, 1); + + float scaling[3]; + + /* calculate axis scaling */ + scaling[0] = 1.16f / avg_excited[0]; + /* second axis inverted */ + scaling[1] = 1.16f / -avg_excited[1]; + scaling[2] = 1.08f / avg_excited[2]; + + warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); + + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + } + + if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) { + warnx("failed to disable sensor calibration mode"); + } + + /* set scaling in device */ + mscale_previous.x_scale = scaling[0]; + mscale_previous.y_scale = scaling[1]; + mscale_previous.z_scale = scaling[2]; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to set new scale / offsets for mag"); + } +} + +int HMC5883::set_excitement(unsigned enable) { int ret; /* arm the excitement strap */ @@ -806,8 +954,10 @@ int HMC5883::calibrate(unsigned enable) ret = read_reg(ADDR_CONF_A, conf_reg); if (OK != ret) perf_count(_comms_errors); - if (enable) { + if (enable < 0) { conf_reg |= 0x01; + } else if (enable > 0) { + conf_reg |= 0x02; } else { conf_reg &= ~0x03; } @@ -1020,7 +1170,6 @@ test() */ int calibrate() { - struct mag_report report; ssize_t sz; int ret; @@ -1029,216 +1178,11 @@ int calibrate() if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - err(1, "immediate read failed"); - - warnx("single read"); - warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - - /* set the queue depth to 10 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); - - /* start the sensor polling at 10 Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) - errx(1, "failed to set 2Hz poll rate"); - - /* Set to 2.5 Gauss */ - if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { - warnx("failed to set 2.5 Ga range"); - } - - if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { warnx("failed to enable sensor calibration mode"); } - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set null scale / offsets for mag"); - } - - float avg_excited[3]; - unsigned i; - - /* read the sensor 10x and report each value */ - for (i = 0; i < 10; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) - errx(1, "timed out waiting for sensor data"); - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - err(1, "periodic read failed"); - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; - } - - warnx("periodic read %u", i); - warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - } - - // warnx("starting calibration"); - - // struct mag_report report; - // ssize_t sz; - // int ret; - - // int fd = open(MAG_DEVICE_PATH, O_RDONLY); - // if (fd < 0) - // err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - - // /* do a simple demand read */ - // sz = read(fd, &report, sizeof(report)); - // if (sz != sizeof(report)) - // err(1, "immediate read failed"); - - // warnx("single read"); - // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - // warnx("time: %lld", report.timestamp); - - // /* get scaling, set to zero */ - // struct mag_scale mscale_previous; - - // if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { - // warn("WARNING: failed to get scale / offsets for mag"); - // } - - // struct mag_scale mscale_null = { - // 0.0f, - // 1.0f, - // 0.0f, - // 1.0f, - // 0.0f, - // 1.0f, - // }; - - // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - // warn("WARNING: failed to set null scale / offsets for mag"); - // } - - // warnx("sensor ready"); - - // float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - - // if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { - // warnx("failed to enable sensor calibration mode"); - // } - - // /* Set to 2.5 Gauss */ - // if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { - // warnx("failed to set 2.5 Ga range"); - // } - - // /* set the queue depth to 10 */ - // if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { - // warnx("failed to set queue depth"); - // return 1; - // } else { - // warnx("set queue depth"); - // } - - // /* start the sensor polling at 100Hz */ - // if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 100)) { - // warnx("failed to set 100 Hz poll rate"); - // return 1; - // } else { - // warnx("set 100 Hz poll rate"); - // } - - // int i; - // for (i = 0; i < 10; i++) { - // struct pollfd fds; - - // (void) ioctl(fd, MAGIOCCALIBRATE, 1); - - // /* wait for data to be ready */ - // fds.fd = fd; - // fds.events = POLLIN; - // ret = poll(&fds, 1, 2000); - - // if (ret != 1) { - // warnx("timed out waiting for sensor data"); - // return 1; - // } - - // /* now go get it */ - // sz = read(fd, &report, sizeof(report)); - - // if (sz != sizeof(report)) { - // warn("periodic read failed"); - // return 1; - // } else { - // avg_excited[0] += report.x; - // avg_excited[1] += report.y; - // avg_excited[2] += report.z; - // } - // warnx("excited read %u", i); - // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - // warnx("time: %lld", report.timestamp); - - // } - - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; - - warnx("periodic excited reads %u", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); - - /* Set to 1.1 Gauss and end calibration */ - ret = ioctl(fd, MAGIOCCALIBRATE, 0); - ret = ioctl(fd, MAGIOCSRANGE, 1); - - float scaling[3]; - - /* calculate axis scaling */ - scaling[0] = 1.16f / avg_excited[0]; - /* second axis inverted */ - scaling[1] = 1.16f / -avg_excited[1]; - scaling[2] = 1.08f / avg_excited[2]; - - warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - - /* set back to normal mode */ - /* Set to 1.1 Gauss */ - if (OK != ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); - } - - if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) { - warnx("failed to disable sensor calibration mode"); - } - - /* set scaling in device */ - // mscale_previous.x_scale = scaling[0]; - // mscale_previous.y_scale = scaling[1]; - // mscale_previous.z_scale = scaling[2]; - - // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - // warn("WARNING: failed to set new scale / offsets for mag"); - // } + close(fd); errx(0, "PASS"); } 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..9a90d7fc58 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"); From 5d3d17d025fa860879b145a99ec80afb7db38edc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 10:38:23 +0200 Subject: [PATCH 18/40] Increased priority of MAVLink receiver thread --- apps/mavlink/mavlink.c | 2 +- apps/mavlink/mavlink_receiver.c | 5 +++++ apps/multirotor_att_control/multirotor_attitude_control.c | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e893ea951c..075dc9e3b7 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1,4 +1,4 @@ -/**************************************************************************** + /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: @author Lorenz Meier diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index ad46c3edea..060bff2b46 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -487,6 +487,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, ¶m); + pthread_attr_setstacksize(&receiveloop_attr, 2048); pthread_t thread; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 00b759d73a..33cab5cc9d 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -248,7 +248,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s att->roll, att->rollspeed, deltaT)*1/10.0f; /* 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),cos(att->yaw - att_sp->yaw_body)); From 23d294453bbdade03a67002f62b898e7aca65a70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:08:33 +0200 Subject: [PATCH 19/40] Fixed a range of initialization issues in filter, does not any more emit NaN in first iteration --- .../attitude_estimator_ekf_main.c | 59 +++++++++---------- .../attitude_estimator_ekf_params.c | 6 +- .../codegen/attitudeKalmanfilter.c | 12 ++-- 3 files changed, 36 insertions(+), 41 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index a291a49146..7bb8490e52 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -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++; diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index f20d1b204b..4fc2e0452a 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -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); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 885c01bf3d..04f6ea267e 100644 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -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]); } From 8b000b331704b2d37a8c47ae1bc480b784965db4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:09:28 +0200 Subject: [PATCH 20/40] Fixed an abort condition, fixed value initialization, implemented naive three-step calibration --- apps/commander/commander.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 43df6ceb28..746c0e7917 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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); @@ -333,13 +339,14 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 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(); @@ -823,7 +830,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, ¤t_status); + ioctl(buzzer, TONE_SET_ALARM, 2); 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; From d4e6a9d7a1e448a5cc55436c0bb5f951694bc46e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:10:04 +0200 Subject: [PATCH 21/40] Minor code style fixes, removed dead code --- apps/mavlink/mavlink_receiver.c | 13 ++----------- apps/mavlink/orb_listener.c | 17 ++++------------- 2 files changed, 6 insertions(+), 24 deletions(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 060bff2b46..15ed70dbde 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -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; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 39358922df..63a39e4ee0 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -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++; } @@ -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); From e4645c0a411b62ebe58c14639f5259e6a444178d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:10:32 +0200 Subject: [PATCH 22/40] Initialized all sensor fields to zero --- apps/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index b84b58406f..2ab1a92038 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -993,6 +993,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; From 2d631fb0059cecdc429c5847e8dbede82348d129 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 18:26:56 +0200 Subject: [PATCH 23/40] Various fixes to attitude control, flyable, needs parameter tuning --- .../multirotor_att_control_main.c | 2 +- .../multirotor_attitude_control.c | 11 ++++++----- .../multirotor_rate_control.c | 14 ++++++++++++-- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index cb82036fe8..82d4c0f253 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -195,7 +195,7 @@ mc_thread_main(int argc, char *argv[]) rates_sp.roll = manual.roll; rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; + rates_sp.yaw = att.yaw + manual.yaw * 2.0f; rates_sp.thrust = manual.throttle; //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 33cab5cc9d..c3da85c5c5 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -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)); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index c132665d21..db3f1a0a69 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -153,6 +153,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,7 +180,8 @@ 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); + printf("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 */ @@ -185,6 +193,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pitch_control_last = pitch_control; } else { pitch_control = 0.0f; + printf("rej. NaN ctrl pitch\n"); } /* control roll (left/right) output */ @@ -194,10 +203,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, roll_control_last = roll_control; } else { roll_control = 0.0f; + printf("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)*p.yawrate_lim-rates[2]); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; From 6a48b91beae575ad4684151f08c75b079caffe5c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 18:27:21 +0200 Subject: [PATCH 24/40] Lowering default rates at 57600 --- apps/mavlink/mavlink.c | 6 +++--- apps/mavlink/orb_listener.c | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 075dc9e3b7..293bbe00c0 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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 */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 63a39e4ee0..c2428874fd 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -551,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)); From d1429f266da33aa3355b228adffe56f74a9c3cd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 18:27:49 +0200 Subject: [PATCH 25/40] Calibration progress, needs sphere fitting --- apps/commander/commander.c | 42 ++++++++++++++++++++------------------ 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 746c0e7917..687b02d4f4 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -325,8 +325,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) { @@ -334,15 +334,17 @@ 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)) { @@ -362,26 +364,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 { From dff0051568602d60f6b8b3d501a2ea78e98b9d7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 19:02:57 +0200 Subject: [PATCH 26/40] Map inputs to the controller we actually want --- apps/multirotor_att_control/multirotor_att_control_main.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 82d4c0f253..dfb778fd0a 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -195,7 +195,7 @@ mc_thread_main(int argc, char *argv[]) rates_sp.roll = manual.roll; rates_sp.pitch = manual.pitch; - rates_sp.yaw = att.yaw + manual.yaw * 2.0f; + rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); @@ -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(); } From 692baa9d07fc9d991a36ffa060540ae9f5b77058 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Oct 2012 13:40:07 +0200 Subject: [PATCH 27/40] Updated MAVLink --- .../v1.0/ardupilotmega/ardupilotmega.h | 6 +- .../mavlink/v1.0/ardupilotmega/version.h | 2 +- mavlink/include/mavlink/v1.0/common/common.h | 9 +- .../v1.0/common/mavlink_msg_battery_status.h | 320 ++++++++++++++++++ .../v1.0/common/mavlink_msg_setpoint_6dof.h | 276 +++++++++++++++ .../v1.0/common/mavlink_msg_setpoint_8dof.h | 320 ++++++++++++++++++ .../include/mavlink/v1.0/common/testsuite.h | 176 ++++++++++ mavlink/include/mavlink/v1.0/common/version.h | 2 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 6 +- .../mavlink/v1.0/matrixpilot/version.h | 2 +- .../include/mavlink/v1.0/pixhawk/pixhawk.h | 6 +- .../include/mavlink/v1.0/pixhawk/version.h | 2 +- .../mavlink/v1.0/sensesoar/sensesoar.h | 6 +- .../include/mavlink/v1.0/sensesoar/version.h | 2 +- 14 files changed, 1115 insertions(+), 20 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h 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 Date: Thu, 18 Oct 2012 13:40:17 +0200 Subject: [PATCH 28/40] Removed dead code --- apps/ardrone_interface/ardrone_interface.c | 1 - 1 file changed, 1 deletion(-) 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); From 5ec5754f26f1059847b7f149bcbdf2b0d11a5115 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Oct 2012 17:34:06 +0200 Subject: [PATCH 29/40] brought controller back to last tuned state --- .../multirotor_rate_control.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index db3f1a0a69..7401ef0f6a 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -52,6 +52,7 @@ #include #include #include +#include #include PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ @@ -180,34 +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("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz\n", + 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; - printf("rej. NaN ctrl pitch\n"); + 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; - printf("rej. NaN ctrl roll\n"); + warnx("rej. NaN ctrl roll\n"); } /* control yaw rate */ - float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.yawrate_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; From 42c61271ea73be29a21110980764d1a68f8ee530 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Oct 2012 19:53:49 +0200 Subject: [PATCH 30/40] remove bogus dt from att rate --- apps/multirotor_att_control/multirotor_attitude_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index c3da85c5c5..a08ee5e46c 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -249,7 +249,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s att->roll, att->rollspeed, deltaT); /* control yaw rate */ - rates_sp->yaw= deltaT * p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)); + rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)); From 096bf2dc93fe8360fa83bee409452f8db7bc3593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Oct 2012 15:36:29 +0200 Subject: [PATCH 31/40] Checkpoint: Working, but non-verified full mag calibration --- apps/commander/calibration_routines.c | 175 +++++++++++++++++++++++ apps/commander/calibration_routines.h | 21 +++ apps/commander/commander.c | 198 +++++++++++++++----------- apps/drivers/hmc5883/hmc5883.cpp | 157 ++++++++++++-------- 4 files changed, 408 insertions(+), 143 deletions(-) create mode 100644 apps/commander/calibration_routines.c create mode 100644 apps/commander/calibration_routines.h 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 545569a651..285b11a457 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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 */ @@ -288,8 +290,9 @@ 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)); + orb_set_interval(sub_mag, 22); + struct mag_report mag; /* 30 seconds */ uint64_t calibration_interval = 45 * 1000 * 1000; @@ -306,8 +309,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) int fd = open(MAG_DEVICE_PATH, 0); - - + /* erase old calibration */ struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -321,8 +323,15 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 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); + /* calibrate offsets */ + uint64_t calibration_start = hrt_absolute_time(); uint64_t axis_deadline = hrt_absolute_time(); @@ -331,10 +340,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const char axislabels[3] = { 'Z', 'X', 'Y'}; int axis_index = -1; - while (hrt_absolute_time() < calibration_deadline) { + const int calibration_maxcount = 2000; + 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 && @@ -363,30 +378,34 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // } 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 */ - /* ignore other axes */ - 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 { @@ -396,76 +415,89 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } } - /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; - float mag_offset[3]; + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - /** - * 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) - * max - max/2 + min/2 - * max/2 + min/2 - * - * which reduces to - * - * offset = (max + min) / 2.0f - */ + free(x); + free(y); + free(z); - 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; + float mag_offset[3] = {sphere_x, sphere_y, sphere_z}; + + // * + // * 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) + // * max - max/2 + min/2 + // * max/2 + min/2 + // * + // * 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; if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) { - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } - - 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"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mag_offset[1]))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(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, - }; + + 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 = mag_offset[0]; + mscale.y_offset = mag_offset[1]; + mscale.z_offset = mag_offset[2]; + 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] scale: %.6f %.6f %.6f\n\t\toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + mscale.x_scale, mscale.y_scale, mscale.z_scale, + mscale.x_offset, mscale.y_offset, mscale.z_offset, sphere_radius); // char buf[50]; // sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000)); @@ -475,7 +507,11 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); } - close(sub_sensor_combined); + /* disable calibration mode */ + status->flag_preflight_mag_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_mag); } void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index fc80c8d177..fc095bff89 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -184,7 +184,7 @@ private: * * @param enable set to 1 to enable self-test strap, 0 to disable */ - int calibrate(unsigned enable); + int calibrate(struct file *filp, unsigned enable); /** * Perform the on-sensor scale calibration routine. @@ -603,7 +603,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 0; case MAGIOCCALIBRATE: - return calibrate(arg); + return calibrate(filp, arg); case MAGIOCEXSTRAP: return set_excitement(arg); @@ -813,41 +813,15 @@ out: return ret; } -int HMC5883::calibrate(unsigned enable) +int HMC5883::calibrate(struct file *filp, unsigned enable) { struct mag_report report; ssize_t sz; - int ret; + int ret = 1; + // XXX do something smarter here int fd = (int)enable; - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - err(1, "immediate read failed"); - - warnx("starting mag scale calibration"); - warnx("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(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); - - /* start the sensor polling at 50 Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50)) - errx(1, "failed to set 2Hz poll rate"); - - /* Set to 2.5 Gauss */ - if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { - warnx("failed to set 2.5 Ga range"); - } - - if (OK != ioctl(fd, MAGIOCPOSEX, 1)) { - warnx("failed to enable sensor calibration mode"); - } - struct mag_scale mscale_previous = { 0.0f, 1.0f, @@ -857,10 +831,6 @@ int HMC5883::calibrate(unsigned enable) 1.0f, }; - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to get scale / offsets for mag"); - } - struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -870,12 +840,61 @@ int HMC5883::calibrate(unsigned enable) 1.0f, }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set null scale / offsets for mag"); + float avg_excited[3] = {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; } - float avg_excited[3]; - unsigned i; + 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++) { @@ -884,56 +903,56 @@ int HMC5883::calibrate(unsigned enable) /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + ret = ::poll(&fds, 1, 2000); - if (ret != 1) - errx(1, "timed out waiting for sensor data"); + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } /* now go get it */ - sz = read(fd, &report, sizeof(report)); + sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "periodic read failed"); + 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); - // warnx("time: %lld", report.timestamp); + //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("periodic excited reads %u", 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]); - /* Set to 1.1 Gauss and end calibration */ - ret = ioctl(fd, MAGIOCNONEX, 0); - ret = ioctl(fd, MAGIOCSRANGE, 1); - float scaling[3]; /* calculate axis scaling */ - scaling[0] = 1.16f / avg_excited[0]; + scaling[0] = fabsf(1.16f / avg_excited[0]); /* second axis inverted */ - scaling[1] = 1.16f / -avg_excited[1]; - scaling[2] = 1.08f / avg_excited[2]; + 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)) { + if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { warnx("failed to set 1.1 Ga range"); + goto out; } - if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) { + if (OK != ioctl(filp, MAGIOCEXSTRAP, 0)) { warnx("failed to disable sensor calibration mode"); + goto out; } /* set scaling in device */ @@ -941,9 +960,20 @@ int HMC5883::calibrate(unsigned enable) mscale_previous.y_scale = scaling[1]; mscale_previous.z_scale = scaling[2]; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + 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("calibration successfully finished."); + } else { + warnx("calibration failed."); + } + return ret; } int HMC5883::set_excitement(unsigned enable) @@ -954,7 +984,7 @@ int HMC5883::set_excitement(unsigned enable) ret = read_reg(ADDR_CONF_A, conf_reg); if (OK != ret) perf_count(_comms_errors); - if (enable < 0) { + if (((int)enable) < 0) { conf_reg |= 0x01; } else if (enable > 0) { conf_reg |= 0x02; @@ -1170,7 +1200,6 @@ test() */ int calibrate() { - struct mag_report report; ssize_t sz; int ret; @@ -1178,13 +1207,17 @@ int calibrate() if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); } close(fd); - errx(0, "PASS"); + if (ret == OK) { + errx(0, "PASS"); + } else { + errx(1, "FAIL"); + } } /** From 1e0a34a10211dbe9894540a45dcbe428d5ae09bd Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 21 Oct 2012 16:39:53 -0600 Subject: [PATCH 32/40] Add functions for computation of the distance and bearing to the nearest point of a line segment or arc segment. --- apps/systemlib/geo/geo.c | 206 ++++++++++++++++++++++++++++++++-- apps/systemlib/geo/geo.h | 23 ++++ nuttx/arch/arm/include/math.h | 4 + 3 files changed, 221 insertions(+), 12 deletions(-) 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/nuttx/arch/arm/include/math.h b/nuttx/arch/arm/include/math.h index ef08692432..9de10c58b4 100644 --- a/nuttx/arch/arm/include/math.h +++ b/nuttx/arch/arm/include/math.h @@ -544,6 +544,8 @@ extern int matherr _PARAMS((struct exception *e)); #define M_1_PI 0.31830988618379067154 #define M_2_PI 0.63661977236758134308 #define M_2_SQRTPI 1.12837916709551257390 +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 #define M_SQRT2 1.41421356237309504880 #define M_SQRT1_2 0.70710678118654752440 #define M_LN2LO 1.9082149292705877000E-10 @@ -568,6 +570,8 @@ extern int matherr _PARAMS((struct exception *e)); #define M_1_PI_F 0.31830988618379067154f #define M_2_PI_F 0.63661977236758134308f #define M_2_SQRTPI_F 1.12837916709551257390f +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f #define M_SQRT2_F 1.41421356237309504880f #define M_SQRT1_2_F 0.70710678118654752440f #define M_LN2LO_F 1.9082149292705877000E-10f From 5925d146bc0457ad0955a939e50eff4c5fe131f8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 21 Oct 2012 17:51:21 -0700 Subject: [PATCH 33/40] Move the tone_alarm driver out of the NuttX configs area and add it as an stm32-specific driver in the PX4 apps space. Add a new tone_alarm command that can be used to start/stop alarm tones from the shell. --- ROMFS/scripts/rcS | 8 +- apps/commander/commander.c | 2 +- .../include => apps/drivers}/drv_tone_alarm.h | 4 +- apps/drivers/stm32/tone_alarm/Makefile | 43 +++ .../drivers/stm32/tone_alarm/tone_alarm.cpp | 365 ++++++++++++------ apps/px4/tests/test_hrt.c | 2 +- nuttx/configs/px4fmu/nsh/appconfig | 1 + nuttx/configs/px4fmu/src/Makefile | 2 +- nuttx/configs/px4fmu/src/up_nsh.c | 6 - 9 files changed, 296 insertions(+), 137 deletions(-) rename {nuttx/configs/px4fmu/include => apps/drivers}/drv_tone_alarm.h (98%) create mode 100644 apps/drivers/stm32/tone_alarm/Makefile rename nuttx/configs/px4fmu/src/drv_tone_alarm.c => apps/drivers/stm32/tone_alarm/tone_alarm.cpp (61%) 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/commander/commander.c b/apps/commander/commander.c index c569a6aa4a..523b24269a 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 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/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..5a04671975 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 int 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/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/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index be0a3d1d71..629d9877fd 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -93,6 +93,7 @@ CONFIGURED_APPS += drivers/hmc5883 CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += drivers/bma180 CONFIGURED_APPS += drivers/l3gd20 +CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += px4/px4io/driver CONFIGURED_APPS += px4/fmu diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 48b528f6a0..338f364fcb 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -43,7 +43,7 @@ AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \ drv_gpio.c \ drv_led.c drv_eeprom.c \ - drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \ + up_pwm_servo.c up_usbdev.c \ up_cpuload.c ifeq ($(CONFIG_NSH_ARCHINIT),y) diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c index af8253181a..763207d3a3 100644 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ b/nuttx/configs/px4fmu/src/up_nsh.c @@ -58,7 +58,6 @@ #include #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; } From c9928c23f31fb6fc8580581c9c62a5f651ad4aff Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 21 Oct 2012 18:22:18 -0700 Subject: [PATCH 34/40] Remove the old rcS template --- nuttx/configs/px4fmu/include/rcS.template | 39 ----------------------- 1 file changed, 39 deletions(-) delete mode 100644 nuttx/configs/px4fmu/include/rcS.template diff --git a/nuttx/configs/px4fmu/include/rcS.template b/nuttx/configs/px4fmu/include/rcS.template deleted file mode 100644 index 2f97a72239..0000000000 --- a/nuttx/configs/px4fmu/include/rcS.template +++ /dev/null @@ -1,39 +0,0 @@ -#echo "---------------------------" -# Start apps -echo "init: Starting applications.." -echo "---------------------------" -# WARNING: -# ttyS0 is ALWAYS the NSH UART -# ttyS1..SN are enumerated according to HW -# uart indices (ttyS1 is the first UART NOT -# configured for NSH, e.g. UART2) -# ttyS0: UART1 -# ttyS1: UART2 -# ttyS2: UART5 -# ttyS3: UART6 -uorb start -mavlink -d /dev/ttyS0 -b 57600 & -echo "Trying to mount microSD card to /fs/microsd.." -if mount -t vfat /dev/mmcsd0 /fs/microsd -then -echo "Successfully mounted SD card." -else -echo "FAILED mounting SD card." -fi -commander & -sensors & -attitude_estimator_bm & -#position_estimator & -ardrone_control -d /dev/ttyS1 -m attitude & -gps -d /dev/ttyS3 -m all & -#sdlog & -#fixedwing_control & -echo "---------------------------" -echo "init: All applications started" -echo "INIT DONE, RUNNING SYSTEM.." - - -# WARNING! USE EXIT ONLY ON AR.DRONE -# NO NSH COMMANDS CAN BE USED AFTER - -exit From 5b9c46977051b2fd7926795b9acd27d8aba572cc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 21 Oct 2012 19:12:32 -0700 Subject: [PATCH 35/40] Function type fix --- apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index 5a04671975..bfde83cde9 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -199,7 +199,7 @@ public: virtual int init(); virtual int ioctl(file *filp, int cmd, unsigned long arg); - virtual int write(file *filp, const char *buffer, size_t len); + virtual ssize_t write(file *filp, const char *buffer, size_t len); private: static const unsigned _max_pattern = 6; From df8148033a1f60400693e80c3732a43cc26e0ee2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Oct 2012 08:14:43 +0200 Subject: [PATCH 36/40] Cleaned up calibration, added text messages ring buffer --- apps/commander/commander.c | 76 +++++++++++++----------------- apps/drivers/hmc5883/hmc5883.cpp | 9 ++-- apps/mavlink/mavlink.c | 29 ++++++++---- apps/mavlink/mavlink_log.c | 80 ++++++++++++++++++++++++++++++++ apps/mavlink/mavlink_log.h | 29 +++++++++++- 5 files changed, 163 insertions(+), 60 deletions(-) create mode 100644 apps/mavlink/mavlink_log.c diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 285b11a457..dafd32ec2b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -291,23 +291,28 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) state_machine_publish(status_pub, status, mavlink_fd); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - orb_set_interval(sub_mag, 22); struct mag_report mag; - /* 30 seconds */ + /* 45 seconds */ uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 2000; unsigned int calibration_counter = 0; - /* - * 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}; + /* 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 = { @@ -332,7 +337,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* calibrate offsets */ - uint64_t calibration_start = hrt_absolute_time(); + // uint64_t calibration_start = hrt_absolute_time(); uint64_t axis_deadline = hrt_absolute_time(); uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; @@ -340,7 +345,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const char axislabels[3] = { 'Z', 'X', 'Y'}; int axis_index = -1; - const int calibration_maxcount = 2000; float *x = malloc(sizeof(float) * calibration_maxcount); float *y = malloc(sizeof(float) * calibration_maxcount); float *z = malloc(sizeof(float) * calibration_maxcount); @@ -426,27 +430,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) free(y); free(z); - float mag_offset[3] = {sphere_x, sphere_y, sphere_z}; - - // * - // * 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) - // * max - max/2 + min/2 - // * max/2 + min/2 - // * - // * 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; - - if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) { + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { fd = open(MAG_DEVICE_PATH, 0); @@ -455,9 +439,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) warn("WARNING: failed to get scale / offsets for mag"); - mscale.x_offset = mag_offset[0]; - mscale.y_offset = mag_offset[1]; - mscale.z_offset = mag_offset[2]; + 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"); @@ -495,13 +479,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: auto-save of params to EEPROM failed"); } - printf("[mag cal] scale: %.6f %.6f %.6f\n\t\toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - mscale.x_scale, mscale.y_scale, mscale.z_scale, - mscale.x_offset, mscale.y_offset, mscale.z_offset, sphere_radius); + 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[50]; - // sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000)); - // mavlink_log_info(mavlink_fd, buf); + 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)"); diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index fc095bff89..8e78825c34 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -284,9 +284,9 @@ HMC5883::HMC5883(int bus) : _next_report(0), _oldest_report(0), _reports(nullptr), - _mag_topic(-1), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), + _mag_topic(-1), _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")) @@ -950,7 +950,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - if (OK != ioctl(filp, MAGIOCEXSTRAP, 0)) { + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { warnx("failed to disable sensor calibration mode"); goto out; } @@ -969,9 +969,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (ret == OK) { - warnx("calibration successfully finished."); + warnx("mag scale calibration successfully finished."); } else { - warnx("calibration failed."); + warnx("mag scale calibration failed."); } return ret; } @@ -1200,7 +1200,6 @@ test() */ int calibrate() { - ssize_t sz; int ret; int fd = open(MAG_DEVICE_PATH, O_RDONLY); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 293bbe00c0..fd02eb3631 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 @@ -114,9 +116,6 @@ mavlink_wpm_storage *wpm = &wpm_s; bool mavlink_hil_enabled = false; -/* buffer for message strings */ -static char mavlink_message_string[51] = {0}; - /* protocol interface */ static int uart; static int baudrate; @@ -127,6 +126,8 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; +static struct mavlink_logbuffer lb; + static void mavlink_update_system(void); static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); static void usage(void); @@ -332,7 +333,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; } @@ -489,6 +492,9 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&lb, 5); + int ch; char *device_name = "/dev/ttyS1"; baudrate = 57600; @@ -662,9 +668,12 @@ int mavlink_thread_main(int argc, char *argv[]) usleep(10000); /* send one string at 10 Hz */ - if (mavlink_message_string[0] != '\0') { - mavlink_missionlib_send_gcs_string(mavlink_message_string); - mavlink_message_string[0] = '\0'; + 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 */ @@ -711,7 +720,7 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, - argv); + (const char**)argv); exit(0); } 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 From a3f2114d5427e1e858a9fb35aa418ba015e95636 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Oct 2012 11:25:26 +0200 Subject: [PATCH 37/40] Removed bogus time scalings --- .../multirotor_rate_control.c | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 7401ef0f6a..75f457fb73 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -55,17 +55,17 @@ #include #include -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ +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, 40.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 40.0f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */ 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, 10.0f); /**< roughly < 500 deg/s limit */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { @@ -181,36 +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); - warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz\n", + 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 */ - // 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); + 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\n"); + warnx("rej. NaN ctrl pitch"); } /* control roll (left/right) output */ - // 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); + 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\n"); + warnx("rej. NaN ctrl roll"); } /* 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; From 3a26708203cbdc5ca8dd0e6b00435f204b9fd2e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Oct 2012 13:28:53 +0200 Subject: [PATCH 38/40] Resolved wrong TX drop display --- apps/commander/commander.c | 6 ++++++ apps/mavlink/mavlink.c | 18 ++++++++++++++++++ apps/mavlink/mavlink_bridge_header.h | 8 +++++++- mavlink/include/mavlink/v1.0/mavlink_helpers.h | 4 ++++ mavlink/include/mavlink/v1.0/protocol.h | 2 ++ 5 files changed, 37 insertions(+), 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index ef508e7591..50d0a7f13a 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -728,6 +728,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 */ @@ -907,6 +910,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; } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index fd02eb3631..2ac803ce07 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -454,6 +454,24 @@ 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; diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h index 8d34c39243..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 @@ -70,4 +73,7 @@ extern mavlink_system_t mavlink_system; */ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int 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/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h index f0f3404a20..f41b36883f 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -12,15 +12,18 @@ /* * Internal function to give access to the channel status for each channel */ +#ifndef MAVLINK_GET_CHANNEL_STATUS MAVLINK_HELPER 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]; } +#endif /* * Internal function to give access to the channel buffer for each channel */ +#ifndef MAVLINK_GET_CHANNEL_BUFFER MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { @@ -35,6 +38,7 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) #endif return &m_mavlink_buffer[chan]; } +#endif /** * @brief Reset the status of a channel. diff --git a/mavlink/include/mavlink/v1.0/protocol.h b/mavlink/include/mavlink/v1.0/protocol.h index 019ae5fc45..64dc222291 100644 --- a/mavlink/include/mavlink/v1.0/protocol.h +++ b/mavlink/include/mavlink/v1.0/protocol.h @@ -42,7 +42,9 @@ #endif // MAVLINK_SEPARATE_HELPERS /* always include the prototypes to ensure we don't get out of sync */ +#ifndef MAVLINK_GET_CHANNEL_STATUS MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan); +#endif MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan); #if MAVLINK_CRC_EXTRA MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, From 9e8a02b928abfd9cd54858f773cc2ab1ffa1e602 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Oct 2012 13:58:13 +0200 Subject: [PATCH 39/40] Switched to a more convenient audio tune --- apps/commander/commander.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 50d0a7f13a..a3dccfd730 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -137,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); @@ -257,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) @@ -364,7 +369,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 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); + tune_confirm(); axis_deadline += calibration_interval / 3; } @@ -838,10 +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"); - ioctl(buzzer, TONE_SET_ALARM, 2); + tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); - ioctl(buzzer, TONE_SET_ALARM, 2); + tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -858,10 +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"); - ioctl(buzzer, TONE_SET_ALARM, 2); + tune_confirm(); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); - ioctl(buzzer, TONE_SET_ALARM, 2); + tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -878,9 +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"); - ioctl(buzzer, TONE_SET_ALARM, 2); + tune_confirm(); do_accel_calibration(status_pub, ¤t_status); - ioctl(buzzer, TONE_SET_ALARM, 2); + 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; @@ -1273,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); From ea36154e3975b12bf72da132e71abdbfb6f5b2bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Oct 2012 16:08:48 +0200 Subject: [PATCH 40/40] Accomodating for offboard control setups --- apps/mavlink/mavlink.c | 7 +- apps/mavlink/mavlink_receiver.c | 6 ++ apps/mavlink/orb_listener.c | 85 +++++++++++-------- .../multirotor_rate_control.c | 4 +- 4 files changed, 61 insertions(+), 41 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 2ac803ce07..698e43f96f 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -119,6 +119,7 @@ bool mavlink_hil_enabled = false; /* protocol interface */ static int uart; static int baudrate; +bool gcs_link = true; /* interface mode */ static enum { @@ -600,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, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200); + 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, 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 { diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 15ed70dbde..5507467949 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -93,6 +93,8 @@ 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) { @@ -218,6 +220,10 @@ handle_message(mavlink_message_t *msg) //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 */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index c2428874fd..0b073cc654 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -61,6 +61,8 @@ #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; @@ -172,15 +174,16 @@ l_sensor_combined(struct listener *l) baro_counter = raw.baro_counter; } - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], - raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], - raw.gyro_rad_s[1], raw.gyro_rad_s[2], - raw.magnetometer_ga[0], - raw.magnetometer_ga[1],raw.magnetometer_ga[2], - raw.baro_pres_mbar, 0 /* no diff pressure yet */, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); + 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++; } @@ -194,8 +197,9 @@ l_vehicle_attitude(struct listener *l) /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); - /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, + if (gcs_link) + /* send sensor values */ + mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, att.roll, att.pitch, @@ -271,8 +275,9 @@ l_rc_channels(struct listener *l) /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, + 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, @@ -322,7 +327,8 @@ l_local_position(struct listener *l) /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, @@ -344,7 +350,8 @@ l_global_position_setpoint(struct listener *l) if (global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, global_sp.lat, global_sp.lon, @@ -360,7 +367,8 @@ l_local_position_setpoint(struct listener *l) /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, @@ -376,7 +384,8 @@ l_attitude_setpoint(struct listener *l) /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, + 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, @@ -399,7 +408,8 @@ l_actuator_outputs(struct listener *l) /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, + 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], @@ -425,7 +435,8 @@ l_manual_control_setpoint(struct listener *l) /* copy manual control data into local buffer */ orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, @@ -441,23 +452,25 @@ l_vehicle_attitude_controls(struct listener *l) orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); + 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) { diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 75f457fb73..42aa0ac636 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -181,8 +181,8 @@ 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); - 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)); + // 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 */