From 9eea4f79d9588153b751b7ba6053840c94b89194 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:58:03 +0400 Subject: [PATCH 001/233] sensors: support for Futaba RC failsafe --- src/modules/sensors/sensor_params.c | 4 ++++ src/modules/sensors/sensors.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b815886..2aa15420af 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -226,3 +226,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); + +PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372c..21da44d101 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -260,6 +260,10 @@ private: float rc_scale_yaw; float rc_scale_flaps; + int rc_fs_ch; + int rc_fs_mode; + float rc_fs_thr; + float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -305,6 +309,10 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; + param_t rc_fs_ch; + param_t rc_fs_mode; + param_t rc_fs_thr; + param_t battery_voltage_scaling; param_t board_rotation; @@ -517,6 +525,11 @@ Sensors::Sensors() : _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); + /* RC failsafe */ + _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); + _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); + _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); @@ -668,6 +681,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); + param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); + param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); + param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -1255,6 +1271,17 @@ Sensors::rc_poll() if (rc_input.channel_count < 4) return; + /* failsafe check */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) + return; + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) + return; + } + } + unsigned channel_limit = rc_input.channel_count; if (channel_limit > _rc_max_chan_count) From 97acb49028da526ad9081e9eecea97ec0a822858 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:58:23 +0400 Subject: [PATCH 002/233] commander: bug fixed in failsafe --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ed090271ca..cbf900fce7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1600,8 +1600,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c /* switch to failsafe mode */ bool manual_control_old = control_mode->flag_control_manual_enabled; - if (!status->condition_landed) { - /* in air: try to hold position */ + if (!status->condition_landed && status->condition_local_position_valid) { + /* in air: try to hold position if possible */ res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); } else { From af12065826060e3ac071869bf39a82695e1d88e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:59:24 +0400 Subject: [PATCH 003/233] sensors: code style fixed --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 21da44d101..3b1460b73b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1276,6 +1276,7 @@ Sensors::rc_poll() if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) return; + } else if (_parameters.rc_fs_mode == 1) { if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) return; From ad133f601bd89156c9cef661cddf684c99ca76cd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 2 Nov 2013 23:33:28 +0400 Subject: [PATCH 004/233] multirotor_att_control: use PID lib for yaw rate control --- .../multirotor_rate_control.c | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index adb63186c0..49fa9986b8 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -168,6 +168,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static PID_t pitch_rate_controller; static PID_t roll_rate_controller; + static PID_t yaw_rate_controller; static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; @@ -182,7 +183,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); } /* load new parameters with lower rate */ @@ -191,13 +192,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f); } /* reset integrals if needed */ if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); - // TODO pid_reset_integral(&yaw_rate_controller); + pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ @@ -208,18 +210,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , rates[0], 0.0f, deltaT); - /* control yaw rate */ //XXX use library here - float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); - - /* increase resilience to faulty control inputs */ - if (!isfinite(yaw_rate_control)) { - yaw_rate_control = 0.0f; - warnx("rej. NaN ctrl yaw"); - } + /* control yaw output */ + float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; + actuators->control[2] = yaw_control; actuators->control[3] = rate_sp->thrust; motor_skip_counter++; From 67c33b28102067db2bfc5240f71f4d2c3eb97b4a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 2 Nov 2013 23:35:53 +0400 Subject: [PATCH 005/233] multirotor_att_control: style fixes, cleanup --- .../multirotor_rate_control.c | 36 +++---------------- 1 file changed, 5 insertions(+), 31 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 49fa9986b8..86ac0e4ff0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -59,31 +59,23 @@ #include #include -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); -//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { float yawrate_p; float yawrate_d; float yawrate_i; - //float yawrate_awu; - //float yawrate_lim; float attrate_p; float attrate_d; float attrate_i; - //float attrate_awu; - //float attrate_lim; float rate_lim; }; @@ -93,14 +85,10 @@ struct mc_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_d; - //param_t yawrate_awu; - //param_t yawrate_lim; param_t attrate_p; param_t attrate_i; param_t attrate_d; - //param_t attrate_awu; - //param_t attrate_lim; }; /** @@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h) 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->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_i = param_find("MC_ATTRATE_I"); h->attrate_d = param_find("MC_ATTRATE_D"); - //h->attrate_awu = param_find("MC_ATTRATE_AWU"); - //h->attrate_lim = param_find("MC_ATTRATE_LIM"); return OK; } @@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru 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->attrate_p, &(p->attrate_p)); param_get(h->attrate_i, &(p->attrate_i)); param_get(h->attrate_d, &(p->attrate_d)); - //param_get(h->attrate_awu, &(p->attrate_awu)); - //param_get(h->attrate_lim, &(p->attrate_lim)); return OK; } @@ -202,15 +182,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_reset_integral(&yaw_rate_controller); } - /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); - - /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); - - /* control yaw output */ + /* run pitch, roll and yaw controllers */ + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); actuators->control[0] = roll_control; From 03162f5f0dd616d7c03f0781cb209e8af9fdae99 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 9 Nov 2013 14:11:39 +0400 Subject: [PATCH 006/233] multirotor_pos_control: failsafe against invalid yaw setpoint in AUTO --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 36dd370fb2..3d23d0c096 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } - att_sp.yaw_body = global_pos_sp.yaw; + /* update yaw setpoint only if value is valid */ + if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { + att_sp.yaw_body = global_pos_sp.yaw; + } mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); From 423e2cee7b2e5d89cb5b7b208788ef5b283b1455 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 10 Nov 2013 15:49:43 +0100 Subject: [PATCH 007/233] Don't limit the PWM output maximum for the IRIS to use the whole thrust range --- ROMFS/px4fmu_common/init.d/16_3dr_iris | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index 6be9178786..f6b071cf1b 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -67,8 +67,7 @@ pwm rate -c 1234 -r 400 # Set disarmed, min and max PWM signals # pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +pwm min -c 1234 -p 1050 # # Start common for all multirotors apps From c29f378e013f20093a1426df22d96be3868c24c6 Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 11 Nov 2013 20:57:03 +0100 Subject: [PATCH 008/233] mkblctrl startup script changed --- ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 4686382ca6..2fd0200a2b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -105,9 +105,12 @@ fi # # Set disarmed, min and max PWM signals # -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +if [ -f /dev/pwm_output ] +then + #pwm disarmed -c 1234 -p 900 + pwm min -c 1234 -p 1200 + pwm max -c 1234 -p 1800 +fi # # Start common for all multirotors apps From 434de4e949c893f53b0d7a3feb1c2588542cd9af Mon Sep 17 00:00:00 2001 From: Thiago0B Date: Mon, 11 Nov 2013 22:02:40 -0200 Subject: [PATCH 009/233] Fix user abort behave in test Now the pwm ouput return to the last value before test (useful and safer when testing ESCs). --- src/systemcmds/pwm/pwm.c | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 09a9344008..898c04cd57 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -395,6 +395,17 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); + /* get current servo values */ + struct pwm_output_values last_spos; + + for (unsigned i = 0; i < servo_count; i++) { + + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); + if (ret != OK) + err(1, "PWM_SERVO_GET(%d)", i); + } + /* perform PWM output */ /* Open console directly to grab CTRL-C signal */ @@ -420,6 +431,14 @@ pwm_main(int argc, char *argv[]) read(0, &c, 1); if (c == 0x03 || c == 0x63 || c == 'q') { + /* reset output to the last value */ + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< Date: Tue, 12 Nov 2013 20:28:26 +0100 Subject: [PATCH 010/233] mkblctrl startup script cleanup --- ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 2fd0200a2b..8e0914d634 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -102,16 +102,6 @@ else mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix fi -# -# Set disarmed, min and max PWM signals -# -if [ -f /dev/pwm_output ] -then - #pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 1200 - pwm max -c 1234 -p 1800 -fi - # # Start common for all multirotors apps # From 185bdb05a69aad9d809e068ea2168df5f7eb0a44 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Nov 2013 22:30:39 +0400 Subject: [PATCH 011/233] Mavlink VFR message publication fix --- src/modules/mavlink/orb_listener.c | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index f6860930c4..abc91d34fa 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -75,6 +75,7 @@ struct actuator_armed_s armed; struct actuator_controls_effective_s actuators_effective_0; struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; +struct airspeed_s airspeed; struct mavlink_subscriptions mavlink_subs; @@ -92,6 +93,8 @@ static unsigned int gps_counter; */ static uint64_t last_sensor_timestamp; +static hrt_abstime last_sent_vfr = 0; + static void *uorb_receive_thread(void *arg); struct listener { @@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l) /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); - if (gcs_link) + if (gcs_link) { /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, @@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l) att.pitchspeed, att.yawspeed); + /* limit VFR message rate to 10Hz */ + hrt_abstime t = hrt_absolute_time(); + if (t >= last_sent_vfr + 100000) { + last_sent_vfr = t; + float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + } + } + attitude_counter++; } @@ -681,17 +695,7 @@ l_home(const struct listener *l) void l_airspeed(const struct listener *l) { - struct airspeed_s airspeed; - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); - - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; - float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.relative_alt; - float climb = -global_pos.vz; - - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } void @@ -849,7 +853,7 @@ uorb_receive_start(void) mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ - /* --- AIRSPEED / VFR / HUD --- */ + /* --- AIRSPEED --- */ mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ From ba3681d3a09df42f5926c1cd1a407d5ed4b34bd6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 12:34:51 -0500 Subject: [PATCH 012/233] Updated backside controller/ added backside config. --- makefiles/config_px4fmu-v1_backside.mk | 160 +++++++++++++++++++ src/modules/fixedwing_backside/fixedwing.hpp | 14 +- 2 files changed, 167 insertions(+), 7 deletions(-) create mode 100644 makefiles/config_px4fmu-v1_backside.mk diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk new file mode 100644 index 0000000000..e1a42530b1 --- /dev/null +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -0,0 +1,160 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4io +MODULES += drivers/px4fmu +MODULES += drivers/boards/px4fmu-v1 +MODULES += drivers/ardrone_interface +MODULES += drivers/l3gd20 +MODULES += drivers/bma180 +MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm +MODULES += drivers/rgbled +MODULES += drivers/mkblctrl +MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += modules/sensors + +# +# System commands +# +MODULES += systemcmds/eeprom +MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/i2c +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard +MODULES += modules/gpio_led + +# +# Estimation modules (EKF / other filters) +# +#MODULES += modules/attitude_estimator_ekf +MODULES += modules/att_pos_estimator_ekf +#MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control +#MODULES += modules/multirotor_att_control +#MODULES += modules/multirotor_pos_control +#MODULES += examples/flow_position_control +#MODULES += examples/flow_speed_control +MODULES += modules/fixedwing_backside + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Unit tests +# +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +#MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) \ + $(call _B, sysinfo, , 2048, sysinfo_main ) diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index 3876e46301..567efeb359 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -255,13 +255,13 @@ private: BlockWaypointGuidance _guide; // block params - BlockParam _trimAil; - BlockParam _trimElv; - BlockParam _trimRdr; - BlockParam _trimThr; - BlockParam _trimV; - BlockParam _vCmd; - BlockParam _crMax; + BlockParamFloat _trimAil; + BlockParamFloat _trimElv; + BlockParamFloat _trimRdr; + BlockParamFloat _trimThr; + BlockParamFloat _trimV; + BlockParamFloat _vCmd; + BlockParamFloat _crMax; struct pollfd _attPoll; vehicle_global_position_set_triplet_s _lastPosCmd; From 5c66899bfbb76cd973f249338507267cdffd0fad Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 15:23:39 -0500 Subject: [PATCH 013/233] Added local position pub to att_pos_esitmator_ekf --- .../att_pos_estimator_ekf/KalmanNav.cpp | 69 ++++++++++++++----- .../att_pos_estimator_ekf/KalmanNav.hpp | 11 ++- 2 files changed, 56 insertions(+), 24 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 18fb6dcbc9..ecca04dd7f 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -41,6 +41,7 @@ #include "KalmanNav.hpp" #include +#include // constants // Titterton pg. 52 @@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // attitude representations C_nb(), q(), - _accel_sub(-1), - _gyro_sub(-1), - _mag_sub(-1), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), + _localPos(&getPublications(), ORB_ID(vehicle_local_position)), _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), @@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), + lat0(0), lon0(0), alt0(0), // parameters for ground station _vGyro(this, "V_GYRO"), _vAccel(this, "V_ACCEL"), @@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : lon = 0.0f; alt = 0.0f; - // gyro, accel and mag subscriptions - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - - struct accel_report accel; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel); - - struct mag_report mag; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag); - // initialize rotation quaternion with a single raw sensor measurement - q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z); + _sensors.update(); + q = init( + _sensors.accelerometer_m_s2[0], + _sensors.accelerometer_m_s2[1], + _sensors.accelerometer_m_s2[2], + _sensors.magnetometer_ga[0], + _sensors.magnetometer_ga[1], + _sensors.magnetometer_ga[2]); // initialize dcm C_nb = Dcm(q); @@ -252,11 +248,21 @@ void KalmanNav::update() setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); + // set reference position for + // local position + lat0 = lat; + lon0 = lon; + alt0 = alt; + // XXX map_projection has internal global + // states that multiple things could change, + // should make map_projection take reference + // lat/lon and not have init + map_projection_init(lat0, lon0); _positionInitialized = true; warnx("initialized EKF state with GPS\n"); warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(vN), double(vE), double(vD), - lat, lon, alt); + lat, lon, double(alt)); } // prediction step @@ -311,7 +317,7 @@ void KalmanNav::updatePublications() { using namespace math; - // position publication + // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; @@ -324,6 +330,31 @@ void KalmanNav::updatePublications() _pos.vz = vD; _pos.yaw = psi; + // local position publication + float x; + float y; + bool landed = alt < (alt0 + 0.1); // XXX improve? + map_projection_project(lat, lon, &x, &y); + _localPos.timestamp = _pubTimeStamp; + _localPos.xy_valid = true; + _localPos.z_valid = true; + _localPos.v_xy_valid = true; + _localPos.v_z_valid = true; + _localPos.x = x; + _localPos.y = y; + _localPos.z = alt0 - alt; + _localPos.vx = vN; + _localPos.vy = vE; + _localPos.vz = vD; + _localPos.yaw = psi; + _localPos.xy_global = true; + _localPos.z_global = true; + _localPos.ref_timestamp = _pubTimeStamp; + _localPos.ref_lat = getLatDegE7(); + _localPos.ref_lon = getLonDegE7(); + _localPos.ref_alt = 0; + _localPos.landed = landed; + // attitude publication _att.timestamp = _pubTimeStamp; _att.roll = phi; @@ -347,8 +378,10 @@ void KalmanNav::updatePublications() // selectively update publications, // do NOT call superblock do-all method - if (_positionInitialized) + if (_positionInitialized) { _pos.update(); + _localPos.update(); + } if (_attitudeInitialized) _att.update(); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 799fc2fb9e..a69bde1a65 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -52,6 +52,7 @@ #include #include +#include #include #include #include @@ -142,12 +143,8 @@ protected: control::UOrbSubscription _param_update; /**< parameter update sub. */ // publications control::UOrbPublication _pos; /**< position pub. */ + control::UOrbPublication _localPos; /**< local position pub. */ control::UOrbPublication _att; /**< attitude pub. */ - - int _accel_sub; /**< Accelerometer subscription */ - int _gyro_sub; /**< Gyroscope subscription */ - int _mag_sub; /**< Magnetometer subscription */ - // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ @@ -164,8 +161,10 @@ protected: float phi, theta, psi; /**< 3-2-1 euler angles */ float vN, vE, vD; /**< navigation velocity, m/s */ double lat, lon; /**< lat, lon radians */ - float alt; /**< altitude, meters */ // parameters + float alt; /**< altitude, meters */ + double lat0, lon0; /**< reference latitude and longitude */ + float alt0; /**< refeerence altitude (ground height) */ control::BlockParamFloat _vGyro; /**< gyro process noise */ control::BlockParamFloat _vAccel; /**< accelerometer process noise */ control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ From ea156f556fe6815e01ea6973bb07de8299851c76 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 15:24:07 -0500 Subject: [PATCH 014/233] Added local position publication to mavlink receiver for HIL. --- src/modules/mavlink/mavlink_receiver.cpp | 65 ++++++++++++++++++++---- 1 file changed, 55 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c51a6de088..bfccd5fb00 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -72,6 +72,7 @@ #include #include #include +#include __BEGIN_DECLS @@ -99,11 +100,13 @@ 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_local_position_s hil_local_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; struct battery_status_s hil_battery_status; static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t pub_hil_local_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; @@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; +// variables for HIL reference position +static int32_t lat0 = 0; +static int32_t lon0 = 0; +static double alt0 = 0; + static void handle_message(mavlink_message_t *msg) { @@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - hil_global_pos.valid = true; - 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(); + uint64_t timestamp = hrt_absolute_time(); + // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + // global position packet + hil_global_pos.timestamp = timestamp; + hil_global_pos.valid = true; + 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; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } + // publish local position + if (pub_hil_local_pos > 0) { + float x; + float y; + bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? + double lat = hil_state.lat*1e-7; + double lon = hil_state.lon*1e-7; + map_projection_project(lat, lon, &x, &y); + hil_local_pos.timestamp = timestamp; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = alt0 - hil_state.alt/1000.0f; + hil_local_pos.vx = hil_state.vx/100.0f; + hil_local_pos.vy = hil_state.vy/100.0f; + hil_local_pos.vz = hil_state.vz/100.0f; + hil_local_pos.yaw = hil_attitude.yaw; + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = hil_state.lat; + hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_alt = alt0; + hil_local_pos.landed = landed; + orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); + } else { + pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + lat0 = hil_state.lat; + lon0 = hil_state.lon; + alt0 = hil_state.alt / 1000.0f; + map_projection_init(hil_state.lat, hil_state.lon); + } + /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); math::Dcm C_nb(q); From 2138a1c816e1856245bf9cb08dcd1304b1f827bb Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 15:24:34 -0500 Subject: [PATCH 015/233] Improved mode mapping for fixedwing_backside. --- src/modules/fixedwing_backside/fixedwing.cpp | 23 ++++++++++---------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d65045d687..dd067e5c43 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? + if (_status.main_state == MAIN_STATE_AUTO) { + // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); } @@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || - _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + if (_status.main_state != MAIN_STATE_AUTO) { - // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between + // calculate velocity, XXX should be airspeed, + // but using ground speed for now for the purpose + // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vx * _pos.vx + @@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update() // a first binary release can be targeted. // This is not a hack, but a design choice. - /* do not limit in HIL */ + // do not limit in HIL if (_status.hil_state != HIL_STATE_ON) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } - } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here? + } else if (_status.main_state == MAIN_STATE_MANUAL) { _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + + } else if (_status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity From 1ffb71946d6291b0e6c49515a07b67e3787ed785 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 16:15:30 -0500 Subject: [PATCH 016/233] Fixed backside automode typo. --- src/modules/fixedwing_backside/fixedwing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index dd067e5c43..6dc19df41f 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -167,7 +167,7 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.main_state != MAIN_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // calculate velocity, XXX should be airspeed, // but using ground speed for now for the purpose From e46d60ba6de3c3809cd7e1e8e1f0485f0290980b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 15 Nov 2013 11:32:05 +0400 Subject: [PATCH 017/233] =?UTF-8?q?px4io=20driver:=20don=E2=80=99t=20use?= =?UTF-8?q?=20PX4IO=5FPAGE=5FACTUATORS=20page=20for=20actuator=5Fcontrols?= =?UTF-8?q?=5Feffective?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/px4io/px4io.cpp | 27 +++++++++++++++++++-------- src/modules/px4iofirmware/mixer.cpp | 6 +++++- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08e697b9f8..1ab18fe7b4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1378,18 +1378,29 @@ PX4IO::io_publish_mixed_controls() actuator_controls_effective_s controls_effective; controls_effective.timestamp = hrt_absolute_time(); - /* get actuator controls from IO */ - uint16_t act[_max_actuators]; - int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + // XXX PX4IO_PAGE_ACTUATORS page contains actuator outputs, not inputs + // need to implement backward mixing in IO's mixed and add another page + // by now simply use control inputs here + /* get actuator outputs from IO */ + //uint16_t act[_max_actuators]; + //int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); - if (ret != OK) - return ret; + //if (ret != OK) + // return ret; /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) - controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + //for (unsigned i = 0; i < _max_actuators; i++) + // controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); - /* laxily advertise on first publication */ + actuator_controls_s controls; + + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &controls); + + for (unsigned i = 0; i < _max_actuators; i++) + controls_effective.control_effective[i] = controls.control[i]; + + /* lazily advertise on first publication */ if (_to_actuators_effective == 0) { _to_actuators_effective = orb_advertise((_primary_pwm_device ? diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 05897b4ce6..35ef5fcf66 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -185,7 +185,7 @@ mixer_tick(void) r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ - r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } @@ -201,6 +201,10 @@ mixer_tick(void) for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + } } if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { From 6ed268aa28dead90d3b78884ecda44df41d32188 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 15 Nov 2013 11:42:19 +0400 Subject: [PATCH 018/233] mavlink: some mavling messages filling bugs fixed --- src/modules/mavlink/orb_listener.c | 47 ++++++++++-------------------- 1 file changed, 15 insertions(+), 32 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34fa..46460d3b8b 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -248,8 +249,8 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; - float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + float throttle = actuators_effective_0.control_effective[3] * 100.0f; mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } } @@ -266,13 +267,7 @@ l_vehicle_gps_position(const struct listener *l) orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); /* GPS COG is 0..2PI in degrees * 1e2 */ - float cog_deg = gps.cog_rad; - - if (cog_deg > M_PI_F) - cog_deg -= 2.0f * M_PI_F; - - cog_deg *= M_RAD_TO_DEG_F; - + float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, @@ -365,28 +360,16 @@ l_global_position(const 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.yaw / 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); + global_pos.timestamp / 1000, + global_pos.lat, + global_pos.lon, + global_pos.alt * 1000.0f, + global_pos.relative_alt * 1000.0f, + global_pos.vx * 100.0f, + global_pos.vy * 100.0f, + global_pos.vz * 100.0f, + _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } void @@ -424,8 +407,8 @@ l_global_position_setpoint(const struct listener *l) coordinate_frame, global_sp.lat, global_sp.lon, - global_sp.altitude, - global_sp.yaw); + global_sp.altitude * 1000.0f, + global_sp.yaw * M_RAD_TO_DEG_F * 100.0f); } void From 45e158b88c1b4dec802c419265d656e706dbe5e6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 16:06:23 +0400 Subject: [PATCH 019/233] Fixed actuator_controls_effective on FMU --- src/drivers/px4fmu/fmu.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e98..6ccb8acdba 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -542,7 +542,7 @@ PX4FMU::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { @@ -589,8 +589,9 @@ PX4FMU::task_main() pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output actual limited values */ - for (unsigned i = 0; i < num_outputs; i++) { - controls_effective.control_effective[i] = (float)pwm_limited[i]; + // XXX copy control values as is, need to do backward mixing of actual limited values properly + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + controls_effective.control_effective[i] = _controls.control[i]; } orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); From 8f559c73e9f3ed2f44aba5fe4fdfbae2542ad8bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 16:07:06 +0400 Subject: [PATCH 020/233] px4io driver: bug fixed --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1ab18fe7b4..2b8a65ae6f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2541,7 +2541,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[4]; /* work out what we're uploading... */ if (argc > 2) { From 39634d100104b64f205b69017562b3ac549cf264 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 16:07:06 +0400 Subject: [PATCH 021/233] px4io driver: bug fixed --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08e697b9f8..ef6ca04e97 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2530,7 +2530,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[4]; /* work out what we're uploading... */ if (argc > 2) { From 63d81ba415302c3ed62b4928e6977b4a5da6767b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 23:16:09 +0400 Subject: [PATCH 022/233] actuator_controls_effective topic removed --- .../ardrone_interface/ardrone_motor_control.c | 23 ------- src/drivers/mkblctrl/mkblctrl.cpp | 14 ---- src/drivers/px4fmu/fmu.cpp | 18 ----- src/drivers/px4io/px4io.cpp | 67 +------------------ src/modules/mavlink/orb_listener.c | 34 +--------- src/modules/sdlog2/sdlog2.c | 21 +----- src/modules/uORB/objects_common.cpp | 7 -- .../uORB/topics/actuator_controls_effective.h | 60 ++++++++--------- 8 files changed, 35 insertions(+), 209 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 01b89c8fa5..81f6349923 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #include "ardrone_motor_control.h" @@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ static bool initialized = false; - /* publish effective outputs */ - static struct actuator_controls_effective_s actuator_controls_effective; - static orb_advert_t actuator_controls_effective_pub; /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { @@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } - - - /* publish effective outputs */ - actuator_controls_effective.control_effective[0] = roll_control; - actuator_controls_effective.control_effective[1] = pitch_control; - /* yaw output after limiting */ - actuator_controls_effective.control_effective[2] = yaw_control; - /* possible motor thrust limiting */ - actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f; - - if (!initialized) { - /* advertise and publish */ - actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective); - initialized = true; - } else { - /* already initialized, just publishing */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective); - } - /* set the motor values */ /* scale up from 0..1 to 10..500) */ diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b93f38cf6b..7ef883f949 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -73,7 +73,6 @@ #include #include -#include #include #include #include @@ -143,7 +142,6 @@ private: int _frametype; orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; orb_advert_t _t_esc_status; unsigned int _num_outputs; @@ -252,7 +250,6 @@ MK::MK(int bus) : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _t_esc_status(0), _num_outputs(0), _motortest(false), @@ -525,13 +522,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - /* advertise the blctrl status */ esc_status_s esc; memset(&esc, 0, sizeof(esc)); @@ -595,9 +585,6 @@ MK::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); outputs.timestamp = hrt_absolute_time(); - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - /* iterate actuators */ for (unsigned int i = 0; i < _num_outputs; i++) { @@ -701,7 +688,6 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6ccb8acdba..39112ab1e0 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,6 @@ #include #include -#include #include #include @@ -123,7 +122,6 @@ private: int _t_actuators; int _t_actuator_armed; orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -219,7 +217,6 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -466,13 +463,6 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -588,13 +578,6 @@ PX4FMU::task_main() pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - /* output actual limited values */ - // XXX copy control values as is, need to do backward mixing of actual limited values properly - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - controls_effective.control_effective[i] = _controls.control[i]; - } - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { up_pwm_servo_set(i, pwm_limited[i]); @@ -651,7 +634,6 @@ PX4FMU::task_main() } ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); /* make sure servos are off */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2b8a65ae6f..0ed30ed919 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,7 +72,6 @@ #include #include -#include #include #include #include @@ -257,14 +256,12 @@ private: /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io - orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; /// #include #include -#include #include #include #include @@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; - struct actuator_controls_effective_s act_controls_effective; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; @@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int rates_sp_sub; int act_outputs_sub; int act_controls_sub; - int act_controls_effective_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; @@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 20; - /* Sanity check variable and index */ + /* number of subscriptions */ + const ssize_t fdsc = 19; + /* sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; @@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.act_controls_effective_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- LOCAL POSITION --- */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; @@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); - // TODO not implemented yet - } - /* --- LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3514dca24d..c6a252b553 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -169,13 +169,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); -/* actuator controls, as set by actuators / mixers after limiting */ -#include "topics/actuator_controls_effective.h" -ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s); - #include "topics/actuator_outputs.h" ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index d7b404ad42..54d84231f0 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -46,34 +46,34 @@ #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H -#include -#include "../uORB.h" -#include "actuator_controls.h" +//#include +//#include "../uORB.h" +//#include "actuator_controls.h" +// +//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS +//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +// +///** +// * @addtogroup topics +// * @{ +// */ +// +//struct actuator_controls_effective_s { +// uint64_t timestamp; +// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; +//}; +// +///** +// * @} +// */ +// +///* actuator control sets; this list can be expanded as more controllers emerge */ +//ORB_DECLARE(actuator_controls_effective_0); +//ORB_DECLARE(actuator_controls_effective_1); +//ORB_DECLARE(actuator_controls_effective_2); +//ORB_DECLARE(actuator_controls_effective_3); +// +///* control sets with pre-defined applications */ +//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) -#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS -#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ - -/** - * @addtogroup topics - * @{ - */ - -struct actuator_controls_effective_s { - uint64_t timestamp; - float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; -}; - -/** - * @} - */ - -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_effective_0); -ORB_DECLARE(actuator_controls_effective_1); -ORB_DECLARE(actuator_controls_effective_2); -ORB_DECLARE(actuator_controls_effective_3); - -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) - -#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ \ No newline at end of file +#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ From 21cc19cef6a6ad9d88ac20cf2223635fe8ec4388 Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 18 Nov 2013 21:32:41 +0100 Subject: [PATCH 023/233] mkblctrl set a default device / -d (device) parameter for alternate device added / -t testmode enhanced --- .../init.d/rc.custom_dji_f330_mkblctrl | 4 +- src/drivers/mkblctrl/mkblctrl.cpp | 73 +++++++++++++------ 2 files changed, 52 insertions(+), 25 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 8e0914d634..40b2ee68bd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -97,9 +97,9 @@ fi # if [ $MKBLCTRL_FRAME == x ] then - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix else - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix fi # diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b93f38cf6b..4e26d9c507 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * Author: Marco Bauer * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +36,8 @@ * @file mkblctrl.cpp * * Driver/configurator for the Mikrokopter BL-Ctrl. - * Marco Bauer + * + * @author Marco Bauer * */ @@ -89,8 +91,8 @@ #define BLCTRL_MIN_VALUE -0.920F #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F -#define MOTOR_SPINUP_COUNTER 2500 -#define ESC_UORB_PUBLISH_DELAY 200 +#define MOTOR_SPINUP_COUNTER 30 +#define ESC_UORB_PUBLISH_DELAY 200 class MK : public device::I2C { @@ -112,7 +114,7 @@ public: FRAME_X, }; - MK(int bus); + MK(int bus, const char *_device_path); ~MK(); virtual int ioctl(file *filp, int cmd, unsigned long arg); @@ -141,6 +143,7 @@ private: unsigned int _motor; int _px4mode; int _frametype; + char _device[20]; ///< device orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; @@ -244,7 +247,7 @@ MK *g_mk; } // namespace -MK::MK(int bus) : +MK::MK(int bus, const char *_device_path) : I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), _mode(MODE_NONE), _update_rate(50), @@ -265,6 +268,10 @@ MK::MK(int bus) : _armed(false), _mixers(nullptr) { + strncpy(_device, _device_path, sizeof(_device)); + /* enforce null termination */ + _device[sizeof(_device) - 1] = '\0'; + _debug_enabled = true; } @@ -291,7 +298,7 @@ MK::~MK() /* clean up the alternate device node */ if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + unregister_driver(_device); g_mk = nullptr; } @@ -313,13 +320,15 @@ MK::init(unsigned motors) usleep(500000); - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (sizeof(_device) > 0) { + ret = register_driver(_device, &fops, 0666, (void *)this); - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } + if (ret == OK) { + log("creating alternate output device"); + _primary_pwm_device = true; + } + + } /* reset GPIOs */ gpio_reset(); @@ -633,10 +642,7 @@ MK::task_main() } /* output to BLCtrl's */ - if (_motortest == true) { - mk_servo_test(i); - - } else { + if (_motortest != true) { //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine // 11 Bit Motor[i].SetPoint_PX4 = outputs.output[i]; @@ -692,9 +698,16 @@ MK::task_main() esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; + + // if motortest is requested - do it... + if (_motortest == true) { + mk_servo_test(i); + } + } orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); + } } @@ -1390,13 +1403,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, } int -mk_start(unsigned bus, unsigned motors) +mk_start(unsigned bus, unsigned motors, char *device_path) { int ret = OK; if (g_mk == nullptr) { - g_mk = new MK(bus); + g_mk = new MK(bus, device_path); if (g_mk == nullptr) { ret = -ENOMEM; @@ -1432,6 +1445,7 @@ mkblctrl_main(int argc, char *argv[]) bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; + char *devicepath = ""; /* * optional parameters @@ -1491,25 +1505,38 @@ mkblctrl_main(int argc, char *argv[]) newMode = true; } + /* look for the optional device parameter */ + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { + if (argc > i + 1) { + devicepath = argv[i + 1]; + newMode = true; + + } else { + errx(1, "missing the devicename (-d)"); + return 1; + } + } + } if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-d devicename] [-t] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); - fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); + fprintf(stderr, "\t -t \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); exit(1); } if (g_mk == nullptr) { - if (mk_start(bus, motorcount) != OK) { + if (mk_start(bus, motorcount, devicepath) != OK) { errx(1, "failed to start the MK-BLCtrl driver"); } else { - newMode = true; + //////newMode = true; } } @@ -1522,5 +1549,5 @@ mkblctrl_main(int argc, char *argv[]) /* test, etc. here g*/ - exit(1); + exit(0); } From 1fa609d165485a834cfc7b648955292f8fe3b86c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 11:44:41 +0100 Subject: [PATCH 024/233] fix off by one in missionlib --- src/modules/mavlink/missionlib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index fa23f996f3..bb857dc690 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, next_setpoint_index = index + 1; } - while (next_setpoint_index < wpm->size - 1) { + while (next_setpoint_index < wpm->size) { if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || From 3527ea8a62e4b4d896cd35bbe9e9c54b0edc1579 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 16:37:48 +0100 Subject: [PATCH 025/233] tecs: fix wrong != 0 check --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 864a9d24d2..a733ef1d2a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time - if (fabsf(_throttle_slewrate) < 0.01f) { + if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; _throttle_dem = constrain(_throttle_dem, From f82a202667c8b4e38d229e6fcac70ca40380aea2 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 19 Nov 2013 17:35:04 +0100 Subject: [PATCH 026/233] actuator effective removed - unused --- src/drivers/mkblctrl/mkblctrl.cpp | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 4e26d9c507..9442ae9064 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -75,7 +75,6 @@ #include #include -#include #include #include #include @@ -146,7 +145,6 @@ private: char _device[20]; ///< device orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; orb_advert_t _t_esc_status; unsigned int _num_outputs; @@ -255,7 +253,6 @@ MK::MK(int bus, const char *_device_path) : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _t_esc_status(0), _num_outputs(0), _motortest(false), @@ -534,13 +531,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - /* advertise the blctrl status */ esc_status_s esc; memset(&esc, 0, sizeof(esc)); @@ -604,9 +594,6 @@ MK::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); outputs.timestamp = hrt_absolute_time(); - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - /* iterate actuators */ for (unsigned int i = 0; i < _num_outputs; i++) { @@ -712,9 +699,8 @@ MK::task_main() } - //::close(_t_esc_status); + ::close(_t_esc_status); ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); From ee985c70b3d774f30d0fbe86f8c7046e49a74104 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Wed, 20 Nov 2013 02:45:52 +0900 Subject: [PATCH 027/233] Update fw_att_control_params.c Minor comment error corrected. --- src/modules/fw_att_control/fw_att_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 97aa275de2..2360c43afb 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file fw_pos_control_l1_params.c + * @file fw_att_control_params.c * * Parameters defined by the L1 position control task * From 9a4b57c352cd11dfd448444d6754c60bd289f5e7 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Wed, 20 Nov 2013 03:04:53 +0900 Subject: [PATCH 028/233] Update fw_att_control_params.c --- src/modules/fw_att_control/fw_att_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 2360c43afb..be76524da3 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -35,7 +35,7 @@ /** * @file fw_att_control_params.c * - * Parameters defined by the L1 position control task + * Parameters defined by the fixed-wing attitude control task * * @author Lorenz Meier */ From cc8e85ce7e4e79a217edd40f64f6870b0ab72bb3 Mon Sep 17 00:00:00 2001 From: marco Date: Thu, 21 Nov 2013 22:24:16 +0100 Subject: [PATCH 029/233] mkblctrl scans now i2c3 and i2c1 bir connected esc's --- src/drivers/mkblctrl/mkblctrl.cpp | 82 ++++++++++++++++++++++++++----- 1 file changed, 70 insertions(+), 12 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 9442ae9064..bd058e5d0a 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -127,7 +127,7 @@ public: int set_overrideSecurityChecks(bool overrideSecurityChecks); int set_px4mode(int px4mode); int set_frametype(int frametype); - unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); + unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: static const unsigned _max_actuators = MAX_MOTORS; @@ -726,8 +726,12 @@ MK::mk_servo_arm(bool status) unsigned int -MK::mk_check_for_blctrl(unsigned int count, bool showOutput) +MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) { + if(initI2C) { + I2C::init(); + } + _retries = 50; uint8_t foundMotorCount = 0; @@ -1366,7 +1370,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* count used motors */ do { - if (g_mk->mk_check_for_blctrl(8, false) != 0) { + if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { shouldStop = 4; } else { @@ -1376,7 +1380,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, sleep(1); } while (shouldStop < 3); - g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); + g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); /* (re)set the PWM output mode */ g_mk->set_mode(servo_mode); @@ -1414,6 +1418,52 @@ mk_start(unsigned bus, unsigned motors, char *device_path) } +int +mk_check_for_i2c_esc_bus(char *device_path, int motors) +{ + int ret; + + if (g_mk == nullptr) { + + g_mk = new MK(3, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 3; + } + + } + + + g_mk = new MK(1, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 1; + } + + } + } + + return -1; +} + + + } // namespace extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); @@ -1424,7 +1474,7 @@ mkblctrl_main(int argc, char *argv[]) PortMode port_mode = PORT_FULL_PWM; int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; - int bus = 1; + int bus = -1; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; @@ -1517,15 +1567,23 @@ mkblctrl_main(int argc, char *argv[]) } - if (g_mk == nullptr) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - - } else { - //////newMode = true; - } + if (bus != -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); } + if (bus != -1) { + if (g_mk == nullptr) { + if (mk_start(bus, motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + + } else { + //////newMode = true; + } + } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + + } /* parameter set ? */ if (newMode) { From d2e32f2fc56d723b566e8e935f86379cec4fee39 Mon Sep 17 00:00:00 2001 From: marco Date: Fri, 22 Nov 2013 21:05:40 +0100 Subject: [PATCH 030/233] mkblctrl - hotfix for i2c scan --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index bd058e5d0a..0a41bfef47 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1567,7 +1567,7 @@ mkblctrl_main(int argc, char *argv[]) } - if (bus != -1) { + if (bus == -1) { bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); } From 69ed7cf91f9f82ce9ed7fadde5fd584e8c0e5c18 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 23 Nov 2013 18:48:05 +0400 Subject: [PATCH 031/233] missionlib: waypoint yaw fixed --- src/modules/mavlink/missionlib.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index bb857dc690..124b3b2ae2 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[last_setpoint_index].z; sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F); set_special_fields(wpm->waypoints[last_setpoint_index].param1, wpm->waypoints[last_setpoint_index].param2, wpm->waypoints[last_setpoint_index].param3, @@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[next_setpoint_index].z; sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F); set_special_fields(wpm->waypoints[next_setpoint_index].param1, wpm->waypoints[next_setpoint_index].param2, wpm->waypoints[next_setpoint_index].param3, @@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, 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; + sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize publication if necessary */ @@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, 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; + sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { From 5f18ce506d1a8395e1565db941b7af64a5936f31 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 24 Nov 2013 13:39:02 +0100 Subject: [PATCH 032/233] Add FrSky telemetry application. This daemon emulates an FrSky sensor hub by periodically sending data packets to an attached FrSky receiver. --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/frsky_telemetry/frsky_data.c | 223 +++++++++++++++ src/drivers/frsky_telemetry/frsky_data.h | 84 ++++++ src/drivers/frsky_telemetry/frsky_telemetry.c | 269 ++++++++++++++++++ src/drivers/frsky_telemetry/module.mk | 41 +++ 6 files changed, 619 insertions(+) create mode 100644 src/drivers/frsky_telemetry/frsky_data.c create mode 100644 src/drivers/frsky_telemetry/frsky_data.h create mode 100644 src/drivers/frsky_telemetry/frsky_telemetry.c create mode 100644 src/drivers/frsky_telemetry/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3068270865..7e53de0ae0 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -37,6 +37,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d6..c5190375f5 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -35,6 +35,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # Needs to be burned to the ground and re-written; for now, diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c new file mode 100644 index 0000000000..8a57070b17 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 frsky_data.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ + +#include "frsky_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; + + +/** + * Initializes the uORB subscriptions. + */ +void frsky_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); +} + +/** + * Sends a 0x5E start/stop byte. + */ +static void +frsky_send_startstop(int uart) +{ + static const uint8_t c = 0x5E; + write(uart, &c, sizeof(c)); +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void +frsky_send_byte(int uart, uint8_t value) +{ + const uint8_t x5E[] = {0x5D, 0x3E}; + const uint8_t x5D[] = {0x5D, 0x3D}; + + switch (value) + { + case 0x5E: + write(uart, x5E, sizeof(x5E)); + break; + + case 0x5D: + write(uart, x5D, sizeof(x5D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +static void +frsky_send_data(int uart, uint8_t id, uint16_t data) +{ + frsky_send_startstop(uart); + + frsky_send_byte(uart, id); + frsky_send_byte(uart, data); /* Low */ + frsky_send_byte(uart, data >> 8); /* High */ +} + +/** + * Sends frame 1 (every 200ms): + * acceleration values, altitude (vario), temperatures, current & voltages, RPM + */ +void frsky_send_frame1(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send formatted frame */ + // TODO + frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius); + frsky_send_data(uart, FRSKY_ID_TEMP2, 0); + + frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ + frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); + + frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v); + frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_RPM, 0); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 2 (every 1000ms): + * course, latitude, longitude, speed, altitude (GPS), fuel level + */ +void frsky_send_frame2(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + // TODO + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + if (global_pos.valid) + { + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + // TODO: latitude, longitude + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + alt = global_pos.alt; + } + + frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, (speed - (int)speed) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, (alt - (int)alt) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_FUEL, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * date, time + */ +void frsky_send_frame3(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + // TODO + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + + frsky_send_startstop(uart); +} diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h new file mode 100644 index 0000000000..e0c39a42bd --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 frsky_data.h + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H + +// FrSky sensor hub data IDs +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +// Public functions +void frsky_init(void); +void frsky_send_frame1(int uart); +void frsky_send_frame2(int uart); +void frsky_send_frame3(int uart); + +#endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c new file mode 100644 index 0000000000..5f3837b6ed --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 frsky_telemetry.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + * This daemon emulates an FrSky sensor hub by periodically sending data + * packets to an attached FrSky receiver. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "frsky_data.h" + + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int frsky_task; + +/* functions */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static void usage(void); +static int frsky_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int frsky_telemetry_main(int argc, char *argv[]); + + +/** + * Opens the UART device and sets all required serial parameters. + */ +static int +frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +{ + /* Open UART */ + const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", uart_name); + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Disable output post-processing */ + uart_config.c_oflag &= ~OPOST; + + /* Set baud rate */ + static const speed_t speed = B9600; + + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + return uart; +} + +/** + * Print command usage information + */ +static void +usage() +{ + fprintf(stderr, "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); + exit(1); +} + +/** + * The daemon thread. + */ +static int +frsky_telemetry_thread_main(int argc, char *argv[]) +{ + /* Default values for arguments */ + char *device_name = "/dev/ttyS1"; /* USART2 */ + + /* Work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + case 'd': + device_name = optarg; + break; + + default: + usage(); + break; + } + } + + /* Print welcome text */ + warnx("FrSky telemetry interface starting..."); + + /* Open UART */ + struct termios uart_config_original; + const int uart = frsky_open_uart(device_name, &uart_config_original); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Subscribe to topics */ + frsky_init(); + + thread_running = true; + + /* Main thread loop */ + unsigned int iteration = 0; + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) + { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) + { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int +frsky_telemetry_main(int argc, char *argv[]) +{ + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "frsky_telemetry already running"); + + thread_should_exit = false; + frsky_task = task_spawn_cmd("frsky_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + frsky_telemetry_thread_main, + (const char **)argv); + + while (!thread_running) { + usleep(200); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "frsky_telemetry already stopped"); + + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk new file mode 100644 index 0000000000..808966691a --- /dev/null +++ b/src/drivers/frsky_telemetry/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# FrSky telemetry application. +# + +MODULE_COMMAND = frsky_telemetry + +SRCS = frsky_data.c \ + frsky_telemetry.c From ea9fcaa27ffc8e1edbb8c7a7dec768208944da3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 13:47:37 +0100 Subject: [PATCH 033/233] update the commander to only use local pos for landing detection when on rotary wing Conflicts: src/modules/commander/commander.cpp --- src/modules/commander/commander.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ace13bb784..dfd4d2f733 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); - if (status.condition_local_altitude_valid) { + if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { + // XXX: only respect the condition_landed when the local position is actually valid + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { return TRANSITION_NOT_CHANGED; } } @@ -1549,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { /* possibly on ground, switch to TAKEOFF if needed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } From 4e713a70835ee041f10d2f34745c9de81973c984 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 26 Nov 2013 19:01:43 +0100 Subject: [PATCH 034/233] motortest mode enhanced --- src/drivers/mkblctrl/mkblctrl.cpp | 67 ++++++++++++++++++------------- 1 file changed, 40 insertions(+), 27 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 0a41bfef47..30d6069b32 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -91,7 +91,7 @@ #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_SPINUP_COUNTER 30 -#define ESC_UORB_PUBLISH_DELAY 200 +#define ESC_UORB_PUBLISH_DELAY 500000 class MK : public device::I2C { @@ -661,7 +661,7 @@ MK::task_main() * Only update esc topic every half second. */ - if (hrt_absolute_time() - esc.timestamp > 500000) { + if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) { esc.counter++; esc.timestamp = hrt_absolute_time(); esc.esc_count = (uint8_t) _num_outputs; @@ -955,6 +955,7 @@ MK::mk_servo_test(unsigned int chan) if (_motor >= _num_outputs) { _motor = -1; _motortest = false; + fprintf(stderr, "[mkblctrl] Motortest finished...\n"); } } @@ -1557,41 +1558,53 @@ mkblctrl_main(int argc, char *argv[]) if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-d devicename] [-t] [--override-security-checks] [-h / --help]\n\n"); - fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); - fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); + fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); + fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); - fprintf(stderr, "\t -t \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "Motortest:\n"); + fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n"); + fprintf(stderr, "mkblctrl -t\n"); + fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); exit(1); } - if (bus == -1) { - bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - } + if (!motortest) { + if (g_mk == nullptr) { + if (bus == -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); + } - if (bus != -1) { - if (g_mk == nullptr) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); + if (bus != -1) { + if (mk_start(bus, motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + } - } else { - //////newMode = true; - } - } - } else { - errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + } - } + exit(0); + } else { + errx(1, "MK-BLCtrl driver already running"); + } - /* parameter set ? */ - if (newMode) { - /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); - } + } else { + if (g_mk == nullptr) { + errx(1, "MK-BLCtrl driver not running. You have to start it first."); - /* test, etc. here g*/ + } else { + g_mk->set_motor_test(motortest); + exit(0); - exit(0); + } + } } From 3c027a8e4d5e5e484a37f1026b6fa7835176426f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 27 Nov 2013 23:04:49 +0400 Subject: [PATCH 035/233] Various HIL-related fixes --- src/modules/mavlink/mavlink_receiver.cpp | 11 ++++------- .../position_estimator_inav_main.c | 11 +++++------ 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bfccd5fb00..7b6fad6589 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -382,16 +382,15 @@ handle_message(mavlink_message_t *msg) /* hil gyro */ static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_counter = hil_counter; /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; static const float mg2ms2 = 9.8f / 1000.0f; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -401,6 +400,7 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + hil_sensors.accelerometer_counter = hil_counter; /* adc */ hil_sensors.adc_voltage_v[0] = 0.0f; @@ -409,7 +409,6 @@ handle_message(mavlink_message_t *msg) /* magnetometer */ float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; @@ -419,15 +418,13 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.magnetometer_counter = hil_counter; /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - - hil_sensors.gyro_counter = hil_counter; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.accelerometer_counter = hil_counter; + hil_sensors.baro_counter = hil_counter; /* differential pressure */ hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 10007bf960..3084b6d928 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - errx(1, "subscriptions poll error on init."); + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter > baro_counter) { + if (wait_baro && sensor.baro_counter != baro_counter) { baro_counter = sensor.baro_counter; /* mean calculation over several measurements */ @@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - warnx("subscriptions poll error."); - thread_should_exit = true; + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); continue; } else if (ret > 0) { @@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accel_counter) { + if (sensor.accelerometer_counter != accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; @@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_updates++; } - if (sensor.baro_counter > baro_counter) { + if (sensor.baro_counter != baro_counter) { baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; From bcd745fb0d32da04efd5ed34252e35dd2d3a3ffd Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Fri, 29 Nov 2013 02:05:15 +0900 Subject: [PATCH 036/233] SO(3) estimator and quaternion receive by mavlink implemented --- makefiles/config_px4fmu-v1_default.mk | 1 + .../attitude_estimator_so3_comp/README | 4 +- .../attitude_estimator_so3_comp_main.cpp | 430 +++++------------- src/modules/mavlink/orb_listener.c | 15 +- 4 files changed, 136 insertions(+), 314 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3068270865..a8aed6bb43 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -72,6 +72,7 @@ MODULES += modules/gpio_led # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3_comp MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3_comp/README index 79c50a5318..02ab66ee43 100644 --- a/src/modules/attitude_estimator_so3_comp/README +++ b/src/modules/attitude_estimator_so3_comp/README @@ -1,5 +1,3 @@ Synopsis - nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200 - -Option -d is for debugging packet. See code for detailed packet structure. + nsh> attitude_estimator_so3_comp start \ No newline at end of file diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 86bda3c75e..e12c0e16ae 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -56,20 +56,16 @@ extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]) static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ + +//! Auxiliary variables to reduce number of repeated operations static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ -static bool bFilterInit = false; - -//! Auxiliary variables to reduce number of repeated operations static float q0q0, q0q1, q0q2, q0q3; static float q1q1, q1q2, q1q3; static float q2q2, q2q3; static float q3q3; - -//! Serial packet related -static int uart; -static int baudrate; +static bool bFilterInit = false; /** * Mainloop of attitude_estimator_so3_comp. @@ -81,15 +77,18 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +/* Function prototypes */ +float invSqrt(float number); +void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz); +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt); + static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d ] [-b ]\n" - "-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n" - "ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n"); + fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n"); exit(1); } @@ -106,12 +105,10 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) if (argc < 1) usage("missing command"); - - if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("attitude_estimator_so3_comp already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -120,20 +117,20 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12400, + 14000, attitude_estimator_so3_comp_thread_main, - (const char **)argv); + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; - while(thread_running){ + while (thread_running){ usleep(200000); - printf("."); } - printf("terminated."); + + warnx("stopped"); exit(0); } @@ -157,7 +154,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) //--------------------------------------------------------------------------------------------------- // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float invSqrt(float number) { +float invSqrt(float number) +{ volatile long i; volatile float x, y; volatile const float f = 1.5F; @@ -221,48 +219,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa q3q3 = q3 * q3; } -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) { +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) +{ float recipNorm; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; - //! Make filter converge to initial solution faster - //! This function assumes you are in static position. - //! WARNING : in case air reboot, this can cause problem. But this is very - //! unlikely happen. - if(bFilterInit == false) - { + // Make filter converge to initial solution faster + // This function assumes you are in static position. + // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. + if(bFilterInit == false) { NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); bFilterInit = true; } //! If magnetometer measurement is available, use it. - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { float hx, hy, hz, bx, bz; float halfwx, halfwy, halfwz; // Normalise magnetometer measurement // Will sqrt work better? PX4 system is powerful enough? - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2); - bx = sqrt(hx * hx + hy * hy); - bz = hz; + // Reference direction of Earth's magnetic field + hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); + hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); + hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); + bx = sqrt(hx * hx + hy * hy); + bz = hz; - // Estimated direction of magnetic field - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); + // Estimated direction of magnetic field + halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); + halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); + halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += (my * halfwz - mz * halfwy); - halfey += (mz * halfwx - mx * halfwz); - halfez += (mx * halfwy - my * halfwx); + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += (my * halfwz - mz * halfwy); + halfey += (mz * halfwx - mx * halfwz); + halfez += (mx * halfwy - my * halfwx); } // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) @@ -293,7 +290,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki gyro_bias[1] += twoKi * halfey * dt; gyro_bias[2] += twoKi * halfez * dt; - gx += gyro_bias[0]; // apply integral feedback + + // apply integral feedback + gx += gyro_bias[0]; gy += gyro_bias[1]; gz += gyro_bias[2]; } @@ -337,208 +336,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl q3 *= recipNorm; // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -void send_uart_byte(char c) -{ - write(uart,&c,1); -} - -void send_uart_bytes(uint8_t *data, int length) -{ - write(uart,data,(size_t)(sizeof(uint8_t)*length)); -} - -void send_uart_float(float f) { - uint8_t * b = (uint8_t *) &f; - - //! Assume float is 4-bytes - for(int i=0; i<4; i++) { - - uint8_t b1 = (b[i] >> 4) & 0x0f; - uint8_t b2 = (b[i] & 0x0f); - - uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10; - uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10; - - send_uart_bytes(&c1,1); - send_uart_bytes(&c2,1); - } -} - -void send_uart_float_arr(float *arr, int length) -{ - for(int i=0;i, default : /dev/ttyS2 - //! -b , default : 115200 - while ((ch = getopt(argc,argv,"d:b:")) != EOF){ - switch(ch){ - case 'b': - baudrate = strtoul(optarg, NULL, 10); - if(baudrate == 0) - printf("invalid baud rate '%s'",optarg); - break; - case 'd': - device_name = optarg; - debug_mode = true; - break; - default: - usage("invalid argument"); - } - } - - if(debug_mode){ - printf("Opening debugging port for 3D visualization\n"); - uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart); - if (uart < 0) - printf("could not open %s", device_name); - else - printf("Open port success\n"); - } - - // print text - printf("Nonlinear SO3 Attitude Estimator initialized..\n\n"); - fflush(stdout); - - int overloadcounter = 19; - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); + warnx("main thread started"); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); @@ -555,8 +389,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -565,17 +399,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + //orb_advert_t att_pub = -1; + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; int printcounter = 0; thread_running = true; - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs @@ -588,9 +420,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* initialize parameter handles */ parameters_init(&so3_comp_param_handles); + parameters_update(&so3_comp_param_handles, &so3_comp_params); uint64_t start_time = hrt_absolute_time(); bool initialized = false; + bool state_initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; @@ -615,12 +449,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { - fprintf(stderr, - "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); + warnx("WARNING: Not getting sensors - sensor app running?"); } - } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -644,11 +475,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds gyro_offsets[2] += raw.gyro_rad_s[2]; offset_count++; - if (hrt_absolute_time() - start_time > 3000000LL) { + if (hrt_absolute_time() > start_time + 3000000l) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; + warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); } } else { @@ -668,9 +500,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[0] = raw.timestamp; } - gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_count[1] != raw.accelerometer_counter) { @@ -696,31 +528,14 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds mag[1] = raw.magnetometer_ga[1]; mag[2] = 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++; - } - - static bool const_initialized = false; - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { - dt = 0.005f; - parameters_update(&so3_comp_param_handles, &so3_comp_params); - const_initialized = true; + if (!state_initialized && dt < 0.05f && dt > 0.001f) { + state_initialized = true; + warnx("state initialized"); } /* do not execute the filter if not initialized */ - if (!const_initialized) { + if (!state_initialized) { continue; } @@ -728,18 +543,23 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. - NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); + NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], + -acc[0], -acc[1], -acc[2], + mag[0], mag[1], mag[2], + so3_comp_params.Kp, + so3_comp_params.Ki, + dt); // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 - Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 - Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 - Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21 - Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 - Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23 - Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31 - Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32 - Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 + Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12 + Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13 + Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21 + Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 + Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23 + Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31 + Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32 + Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 //1-2-3 Representation. //Equation (290) @@ -747,29 +567,42 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll euler[1] = -asinf(Rot_matrix[2]); //! Pitch - euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw + euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - /* Do something */ + // Publish only finite euler angles + att.roll = euler[0] - so3_comp_params.roll_off; + att.pitch = euler[1] - so3_comp_params.pitch_off; + att.yaw = euler[2] - so3_comp_params.yaw_off; } else { /* due to inputs or numerical failure the output is invalid, skip it */ + // Due to inputs or numerical failure the output is invalid + warnx("infinite euler angles, rotation matrix:"); + warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); + warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); + warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + // Don't publish anything continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp > last_data + 12000) { + warnx("sensor data missed"); + } last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; + + // Quaternion + att.q[0] = q0; + att.q[1] = q1; + att.q[2] = q2; + att.q[3] = q3; + att.q_valid = true; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - so3_comp_params.roll_off; - att.pitch = euler[1] - so3_comp_params.pitch_off; - att.yaw = euler[2] - so3_comp_params.yaw_off; - - //! Euler angle rate. But it needs to be investigated again. + // Euler angle rate. But it needs to be investigated again. /* att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); @@ -783,53 +616,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = 0; att.yawacc = 0; - //! Quaternion - att.q[0] = q0; - att.q[1] = q1; - att.q[2] = q2; - att.q[3] = q3; - att.q_valid = true; - /* TODO: Bias estimation required */ memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - + + // Publish + if (att_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); + orb_advertise(ORB_ID(vehicle_attitude), &att); } perf_end(so3_comp_loop_perf); - - //! This will print out debug packet to visualization software - if(debug_mode) - { - float quat[4]; - quat[0] = q0; - quat[1] = q1; - quat[2] = q2; - quat[3] = q3; - send_uart_float_arr(quat,4); - send_uart_byte('\n'); - } } } } loopcounter++; - }// while + } thread_running = false; - /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); - return 0; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34fa..564bf806a5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -242,7 +242,7 @@ l_vehicle_attitude(const struct listener *l) att.rollspeed, att.pitchspeed, att.yawspeed); - + /* limit VFR message rate to 10Hz */ hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { @@ -252,6 +252,19 @@ l_vehicle_attitude(const struct listener *l) float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } + + /* send quaternion values if it exists */ + if(att.q_valid) { + mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.q[0], + att.q[1], + att.q[2], + att.q[3], + att.rollspeed, + att.pitchspeed, + att.yawspeed); + } } attitude_counter++; From b3f1adc54bac36b8da16651a545835bb1877f883 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Fri, 29 Nov 2013 02:35:49 +0900 Subject: [PATCH 037/233] SO3 estimator code has been cleaned --- makefiles/config_px4fmu-v1_backside.mk | 3 +- makefiles/config_px4fmu-v1_default.mk | 4 +- makefiles/config_px4fmu-v2_default.mk | 3 +- .../README | 0 .../attitude_estimator_so3_main.cpp} | 71 +++++++++++---- .../attitude_estimator_so3_params.c | 86 +++++++++++++++++++ .../attitude_estimator_so3_params.h | 67 +++++++++++++++ src/modules/attitude_estimator_so3/module.mk | 8 ++ .../attitude_estimator_so3_comp_params.c | 63 -------------- .../attitude_estimator_so3_comp_params.h | 44 ---------- .../attitude_estimator_so3_comp/module.mk | 8 -- 11 files changed, 219 insertions(+), 138 deletions(-) rename src/modules/{attitude_estimator_so3_comp => attitude_estimator_so3}/README (100%) rename src/modules/{attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp => attitude_estimator_so3/attitude_estimator_so3_main.cpp} (87%) create mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c create mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h create mode 100644 src/modules/attitude_estimator_so3/module.mk delete mode 100755 src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c delete mode 100755 src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h delete mode 100644 src/modules/attitude_estimator_so3_comp/module.mk diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index e1a42530b1..5ecf93a3a1 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -69,12 +69,13 @@ MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # #MODULES += modules/attitude_estimator_ekf MODULES += modules/att_pos_estimator_ekf #MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator +MODULES += modules/attitude_estimator_so3 # # Vehicle Control diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a8aed6bb43..25c22f1fdf 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -69,10 +69,10 @@ MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3_comp +MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d6..015a7387a6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -68,9 +68,10 @@ MODULES += modules/mavlink MODULES += modules/mavlink_onboard # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3/README similarity index 100% rename from src/modules/attitude_estimator_so3_comp/README rename to src/modules/attitude_estimator_so3/README diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp similarity index 87% rename from src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp rename to src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e12c0e16ae..e79726613d 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -1,16 +1,49 @@ -/* - * Author: Hyon Lim +/**************************************************************************** * - * @file attitude_estimator_so3_comp_main.c + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * 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 attitude_estimator_so3_main.cpp * * Implementation of nonlinear complementary filters on the SO(3). * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * + * * Theory of nonlinear complementary filters on the SO(3) is based on [1]. * Quaternion realization of [1] is based on [2]. * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * + * * References * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 @@ -46,16 +79,16 @@ #ifdef __cplusplus extern "C" { #endif -#include "attitude_estimator_so3_comp_params.h" +#include "attitude_estimator_so3_params.h" #ifdef __cplusplus } #endif -extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]); +extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ +static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */ //! Auxiliary variables to reduce number of repeated operations static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ @@ -68,9 +101,9 @@ static float q3q3; static bool bFilterInit = false; /** - * Mainloop of attitude_estimator_so3_comp. + * Mainloop of attitude_estimator_so3. */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); +int attitude_estimator_so3_thread_main(int argc, char *argv[]); /** * Print the correct usage. @@ -88,19 +121,19 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n"); + fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n"); exit(1); } /** - * The attitude_estimator_so3_comp app only briefly exists to start + * The attitude_estimator_so3 app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. * * The actual stack size should be set in the call * to task_create(). */ -int attitude_estimator_so3_comp_main(int argc, char *argv[]) +int attitude_estimator_so3_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -114,11 +147,11 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", + attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, - attitude_estimator_so3_comp_thread_main, + attitude_estimator_so3_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -356,7 +389,7 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) +int attitude_estimator_so3_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds @@ -415,8 +448,8 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_so3_comp_params so3_comp_params; - struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles; + struct attitude_estimator_so3_params so3_comp_params; + struct attitude_estimator_so3_param_handles so3_comp_param_handles; /* initialize parameter handles */ parameters_init(&so3_comp_param_handles); @@ -430,7 +463,7 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) unsigned offset_count = 0; /* register the perf counter */ - perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp"); + perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3"); /* Main loop*/ while (!thread_should_exit) { diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c new file mode 100755 index 0000000000..0c8d522b4d --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * 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 attitude_estimator_so3_params.c + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include "attitude_estimator_so3_params.h" + +/* This is filter gain for nonlinear SO3 complementary filter */ +/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. + Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. + If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which + will compensate gyro bias which depends on temperature and vibration of your vehicle */ +PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. + //! You can set this gain higher if you want more fast response. + //! But note that higher gain will give you also higher overshoot. +PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) + //! This gain is depend on your vehicle status. + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_so3_param_handles *h) +{ + /* Filter gain parameters */ + h->Kp = param_find("SO3_COMP_KP"); + h->Ki = param_find("SO3_COMP_KI"); + + /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ + h->roll_off = param_find("SO3_ROLL_OFFS"); + h->pitch_off = param_find("SO3_PITCH_OFFS"); + h->yaw_off = param_find("SO3_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p) +{ + /* Update filter gain */ + param_get(h->Kp, &(p->Kp)); + param_get(h->Ki, &(p->Ki)); + + /* Update attitude offset */ + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + + return OK; +} diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h new file mode 100755 index 0000000000..dfb4cad05a --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * 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 attitude_estimator_so3_params.h + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include + +struct attitude_estimator_so3_params { + float Kp; + float Ki; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_so3_param_handles { + param_t Kp, Ki; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_so3_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p); diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk new file mode 100644 index 0000000000..e29bb16a62 --- /dev/null +++ b/src/modules/attitude_estimator_so3/module.mk @@ -0,0 +1,8 @@ +# +# Attitude estimator (Nonlinear SO(3) complementary Filter) +# + +MODULE_COMMAND = attitude_estimator_so3 + +SRCS = attitude_estimator_so3_main.cpp \ + attitude_estimator_so3_params.c diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c deleted file mode 100755 index f962515dfb..0000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Author: Hyon Lim - * - * @file attitude_estimator_so3_comp_params.c - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include "attitude_estimator_so3_comp_params.h" - -/* This is filter gain for nonlinear SO3 complementary filter */ -/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. - Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. - If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which - will compensate gyro bias which depends on temperature and vibration of your vehicle */ -PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. - //! You can set this gain higher if you want more fast response. - //! But note that higher gain will give you also higher overshoot. -PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) - //! This gain is depend on your vehicle status. - -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); - -int parameters_init(struct attitude_estimator_so3_comp_param_handles *h) -{ - /* Filter gain parameters */ - h->Kp = param_find("SO3_COMP_KP"); - h->Ki = param_find("SO3_COMP_KI"); - - /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); - - return OK; -} - -int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p) -{ - /* Update filter gain */ - param_get(h->Kp, &(p->Kp)); - param_get(h->Ki, &(p->Ki)); - - /* Update attitude offset */ - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - - return OK; -} diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h deleted file mode 100755 index f006956308..0000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Author: Hyon Lim - * - * @file attitude_estimator_so3_comp_params.h - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include - -struct attitude_estimator_so3_comp_params { - float Kp; - float Ki; - float roll_off; - float pitch_off; - float yaw_off; -}; - -struct attitude_estimator_so3_comp_param_handles { - param_t Kp, Ki; - param_t roll_off, pitch_off, yaw_off; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct attitude_estimator_so3_comp_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p); diff --git a/src/modules/attitude_estimator_so3_comp/module.mk b/src/modules/attitude_estimator_so3_comp/module.mk deleted file mode 100644 index 92f43d9202..0000000000 --- a/src/modules/attitude_estimator_so3_comp/module.mk +++ /dev/null @@ -1,8 +0,0 @@ -# -# Attitude estimator (Nonlinear SO3 complementary Filter) -# - -MODULE_COMMAND = attitude_estimator_so3_comp - -SRCS = attitude_estimator_so3_comp_main.cpp \ - attitude_estimator_so3_comp_params.c From 3701a02a37ddd9e78fa2ddcb7b9e9ec0d136ae79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 Nov 2013 10:00:33 +0100 Subject: [PATCH 038/233] Tests for all PWM pins --- src/systemcmds/pwm/pwm.c | 2 +- src/systemcmds/tests/module.mk | 5 +- .../tests/{tests_file.c => test_file.c} | 2 +- .../tests/{tests_param.c => test_param.c} | 2 +- src/systemcmds/tests/test_ppm_loopback.c | 154 ++++++++++++++++++ src/systemcmds/tests/test_sensors.c | 98 +++++++++-- src/systemcmds/tests/test_servo.c | 66 +++----- src/systemcmds/tests/test_uart_loopback.c | 70 ++------ src/systemcmds/tests/tests.h | 9 +- src/systemcmds/tests/tests_main.c | 14 +- 10 files changed, 297 insertions(+), 125 deletions(-) rename src/systemcmds/tests/{tests_file.c => test_file.c} (99%) rename src/systemcmds/tests/{tests_param.c => test_param.c} (99%) create mode 100644 src/systemcmds/tests/test_ppm_loopback.c diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 898c04cd57..7c23f38c20 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -210,7 +210,7 @@ pwm_main(int argc, char *argv[]) err(1, "PWM_SERVO_GET_COUNT"); if (!strcmp(argv[1], "arm")) { - /* tell IO that its ok to disable its safety with the switch */ + /* tell safety that its ok to disable it with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) err(1, "PWM_SERVO_SET_ARM_OK"); diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6729ce4ae3..5d5fe50d33 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,7 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ - tests_file.c \ + test_file.c \ tests_main.c \ - tests_param.c + test_param.c \ + test_ppm_loopback.c diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/test_file.c similarity index 99% rename from src/systemcmds/tests/tests_file.c rename to src/systemcmds/tests/test_file.c index 7f3a654bf7..798724cf1a 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/test_file.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file tests_file.c + * @file test_file.c * * File write test. */ diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/test_param.c similarity index 99% rename from src/systemcmds/tests/tests_param.c rename to src/systemcmds/tests/test_param.c index 13f17bc436..318d2505b2 100644 --- a/src/systemcmds/tests/tests_param.c +++ b/src/systemcmds/tests/test_param.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file tests_param.c + * @file test_param.c * * Tests related to the parameter system. */ diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c new file mode 100644 index 0000000000..6d4e45c4ca --- /dev/null +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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. + * + ****************************************************************************/ + +/** + * @file test_ppm_loopback.c + * Tests the PWM outputs and PPM input + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "tests.h" + +#include +#include + +int test_ppm_loopback(int argc, char *argv[]) +{ + + int servo_fd, result; + servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; + servo_position_t pos; + + servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); + + if (servo_fd < 0) { + printf("failed opening /dev/pwm_servo\n"); + } + + result = read(servo_fd, &data, sizeof(data)); + + if (result != sizeof(data)) { + printf("failed bulk-reading channel values\n"); + } + + printf("Servo readback, pairs of values should match defaults\n"); + + unsigned servo_count; + result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { + warnx("PWM_SERVO_GET_COUNT"); + return ERROR; + } + + for (unsigned i = 0; i < servo_count; i++) { + result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos); + + if (result < 0) { + printf("failed reading channel %u\n", i); + } + + printf("%u: %u %u\n", i, pos, data[i]); + + } + + /* tell safety that its ok to disable it with the switch */ + result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + /* tell output device that the system is armed (it will output values if safety is off) */ + result = ioctl(servo_fd, PWM_SERVO_ARM, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_ARM"); + + int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; + + + printf("Advancing channel 0 to 1100\n"); + result = ioctl(servo_fd, PWM_SERVO_SET(0), 1100); + printf("Advancing channel 1 to 1900\n"); + result = ioctl(servo_fd, PWM_SERVO_SET(1), 1900); + printf("Advancing channel 2 to 1200\n"); + result = ioctl(servo_fd, PWM_SERVO_SET(2), 1200); + + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + if (result) { + (void)close(servo_fd); + return ERROR; + } + } + + /* give driver 10 ms to propagate */ + usleep(10000); + + /* open PPM input and expect values close to the output values */ + + int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); + + struct rc_input_values rc; + result = read(ppm_fd, &rc, sizeof(rc)); + + if (result != sizeof(rc)) { + warnx("Error reading RC output"); + (void)close(servo_fd); + (void)close(ppm_fd); + return ERROR; + } + + /* go and check values */ + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_GET(i), pwm_values[i]); + if (result) { + (void)close(servo_fd); + return ERROR; + } + } + + return 0; +} diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 14503276cf..f6415b72f2 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012, 2013 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 @@ -49,6 +48,8 @@ #include #include #include +#include +#include #include @@ -77,6 +78,7 @@ static int accel(int argc, char *argv[]); static int gyro(int argc, char *argv[]); static int mag(int argc, char *argv[]); static int baro(int argc, char *argv[]); +static int mpu6k(int argc, char *argv[]); /**************************************************************************** * Private Data @@ -91,6 +93,7 @@ struct { {"gyro", "/dev/gyro", gyro}, {"mag", "/dev/mag", mag}, {"baro", "/dev/baro", baro}, + {"mpu6k", "/dev/mpu6k", mpu6k}, {NULL, NULL, NULL} }; @@ -133,23 +136,83 @@ accel(int argc, char *argv[]) printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } - // /* wait at least 10ms, sensor should have data after no more than 2ms */ - // usleep(100000); - - // ret = read(fd, buf, sizeof(buf)); - - // if (ret != sizeof(buf)) { - // printf("\tMPU-6000: read2 fail (%d)\n", ret); - // return ERROR; - - // } else { - // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); - // } - - /* XXX more tests here */ + if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { + warnx("MPU6K acceleration values out of range!"); + return ERROR; + } /* Let user know everything is ok */ printf("\tOK: ACCEL passed all tests successfully\n"); + close(fd); + + return OK; +} + +static int +mpu6k(int argc, char *argv[]) +{ + printf("\tMPU6K: test start\n"); + fflush(stdout); + + int fd; + struct accel_report buf; + struct gyro_report gyro_buf; + int ret; + + fd = open("/dev/accel_mpu6k", O_RDONLY); + + if (fd < 0) { + printf("\tMPU6K: open fail, run first.\n"); + return ERROR; + } + + /* wait at least 100ms, sensor should have data after no more than 20ms */ + usleep(100000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tMPU6K: read1 fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); + } + + if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { + warnx("MPU6K acceleration values out of range!"); + return ERROR; + } + + /* Let user know everything is ok */ + printf("\tOK: MPU6K ACCEL passed all tests successfully\n"); + + close(fd); + fd = open("/dev/gyro_mpu6k", O_RDONLY); + + if (fd < 0) { + printf("\tMPU6K GYRO: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &gyro_buf, sizeof(gyro_buf)); + + if (ret != sizeof(gyro_buf)) { + printf("\tMPU6K GYRO: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z); + } + + /* Let user know everything is ok */ + printf("\tOK: MPU6K GYRO passed all tests successfully\n"); + close(fd); return OK; } @@ -187,6 +250,7 @@ gyro(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); + close(fd); return OK; } @@ -224,6 +288,7 @@ mag(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); + close(fd); return OK; } @@ -261,6 +326,7 @@ baro(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: BARO passed all tests successfully\n"); + close(fd); return OK; } diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index f95760ca8e..ef8512bf54 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -1,7 +1,6 @@ /**************************************************************************** - * px4/sensors/test_hrt.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -13,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -32,9 +31,11 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_servo.c + * Tests the servo outputs + * + */ #include @@ -55,39 +56,6 @@ #include "tests.h" -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_servo - ****************************************************************************/ - int test_servo(int argc, char *argv[]) { int fd, result; @@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[]) printf("Servo readback, pairs of values should match defaults\n"); - for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { + unsigned servo_count; + result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { + warnx("PWM_SERVO_GET_COUNT"); + return ERROR; + } + + for (unsigned i = 0; i < servo_count; i++) { result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos); if (result < 0) { @@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[]) } - printf("Servos arming at default values\n"); + /* tell safety that its ok to disable it with the switch */ + result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + /* tell output device that the system is armed (it will output values if safety is off) */ result = ioctl(fd, PWM_SERVO_ARM, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_ARM"); + usleep(5000000); printf("Advancing channel 0 to 1500\n"); result = ioctl(fd, PWM_SERVO_SET(0), 1500); + printf("Advancing channel 1 to 1800\n"); + result = ioctl(fd, PWM_SERVO_SET(1), 1800); out: return 0; } diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index 3be152004d..f1d41739be 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -1,8 +1,6 @@ /**************************************************************************** - * px4/sensors/test_gpio.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Lorenz Meier + * Copyright (c) 2012, 2013 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 @@ -14,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -33,9 +31,11 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_uart_loopback.c + * Tests the uart outputs + * + */ #include @@ -55,40 +55,6 @@ #include #include - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_led - ****************************************************************************/ - int test_uart_loopback(int argc, char *argv[]) { @@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[]) int uart5_nwrite = 0; int uart2_nwrite = 0; - int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); // + /* opening stdout */ + int stdout_fd = 1; - /* assuming NuttShell is on UART1 (/dev/ttyS0) */ - int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); // - int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); // + int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); + int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); if (uart2 < 0) { printf("ERROR opening UART2, aborting..\n"); @@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[]) exit(uart5); } - uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'}; + uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'}; uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; @@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[]) for (i = 0; i < 1000; i++) { // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* uart2 -> uart5 */ r = write(uart2, sample_uart2, sizeof(sample_uart2)); @@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[]) uart2_nwrite += r; // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* uart2 -> uart5 */ r = write(uart5, sample_uart5, sizeof(sample_uart5)); @@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[]) uart5_nwrite += r; // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* try to read back values */ do { @@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[]) } while (r > 0); // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); do { r = read(uart2, sample_uart5, sizeof(sample_uart5)); @@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[]) } while (r > 0); // printf("TEST #%d\n",i); -// write(uart1, sample_uart1, sizeof(sample_uart5)); +// write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5)); } for (i = 0; i < 200000; i++) { @@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[]) } - close(uart1); + close(stdout_fd); close(uart2); close(uart5); diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c483108cf0..5cbc5ad88f 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -34,6 +34,12 @@ #ifndef __APPS_PX4_TESTS_H #define __APPS_PX4_TESTS_H +/** + * @file tests.h + * Tests declaration file. + * + */ + /**************************************************************************** * Included Files ****************************************************************************/ @@ -88,6 +94,7 @@ extern int test_int(int argc, char *argv[]); extern int test_float(int argc, char *argv[]); extern int test_ppm(int argc, char *argv[]); extern int test_servo(int argc, char *argv[]); +extern int test_ppm_loopback(int argc, char *argv[]); extern int test_uart_loopback(int argc, char *argv[]); extern int test_uart_baudchange(int argc, char *argv[]); extern int test_cpuload(int argc, char *argv[]); diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9eb9c632ec..cd998dd18d 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (c) 2012, 2013 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 @@ -35,6 +34,8 @@ /** * @file tests_main.c * Tests main file, loads individual tests. + * + * @author Lorenz Meier */ #include @@ -57,14 +58,6 @@ #include "tests.h" -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - /**************************************************************************** * Private Function Prototypes ****************************************************************************/ @@ -94,6 +87,7 @@ const struct { {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST}, {"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST}, {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST}, {"adc", test_adc, OPT_NOJIGTEST}, {"jig_voltages", test_jig_voltages, OPT_NOALLTEST}, {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST}, From a46042754f0afb44bde097468083df309b1f94cd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 10:06:43 +1100 Subject: [PATCH 039/233] lsm303d: added 'lsm303d regdump' command useful for diagnosing issues --- src/drivers/lsm303d/lsm303d.cpp | 53 ++++++++++++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 60601e22c8..c21a255223 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -201,6 +201,11 @@ public: */ void print_info(); + /** + * dump register values + */ + void print_registers(); + protected: virtual int probe(); @@ -1380,6 +1385,30 @@ LSM303D::print_info() _mag_reports->print_info("mag reports"); } +void +LSM303D::print_registers() +{ + const struct { + uint8_t reg; + const char *name; + } regmap[] = { + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + }; + for (uint8_t i=0; iprint_registers(); + + exit(0); +} + } // namespace @@ -1634,5 +1679,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "info")) lsm303d::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "regdump")) + lsm303d::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From 72c53b6537a21671e6f66c27bb779c8ba59a3d7f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:17:53 +1100 Subject: [PATCH 040/233] lsm303d: define some more register addresses --- src/drivers/lsm303d/lsm303d.cpp | 47 +++++++++++++++++++++++++-------- 1 file changed, 36 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c21a255223..de227974ec 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -82,19 +82,24 @@ static const int ERROR = -1; /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 +#define WHO_I_AM 0x49 -#define ADDR_OUT_L_T 0x05 -#define ADDR_OUT_H_T 0x06 -#define ADDR_STATUS_M 0x07 -#define ADDR_OUT_X_L_M 0x08 -#define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x0A -#define ADDR_OUT_Y_H_M 0x0B -#define ADDR_OUT_Z_L_M 0x0C -#define ADDR_OUT_Z_H_M 0x0D +#define ADDR_OUT_TEMP_L 0x05 +#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_INT_CTRL_M 0x12 +#define ADDR_INT_SRC_M 0x13 +#define ADDR_REFERENCE_X 0x1c +#define ADDR_REFERENCE_Y 0x1d +#define ADDR_REFERENCE_Z 0x1e -#define ADDR_OUT_TEMP_A 0x26 #define ADDR_STATUS_A 0x27 #define ADDR_OUT_X_L_A 0x28 #define ADDR_OUT_X_H_A 0x29 @@ -112,6 +117,26 @@ static const int ERROR = -1; #define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 +#define ADDR_FIFO_CTRL 0x2e +#define ADDR_FIFO_SRC 0x2f + +#define ADDR_IG_CFG1 0x30 +#define ADDR_IG_SRC1 0x31 +#define ADDR_IG_THS1 0x32 +#define ADDR_IG_DUR1 0x33 +#define ADDR_IG_CFG2 0x34 +#define ADDR_IG_SRC2 0x35 +#define ADDR_IG_THS2 0x36 +#define ADDR_IG_DUR2 0x37 +#define ADDR_CLICK_CFG 0x38 +#define ADDR_CLICK_SRC 0x39 +#define ADDR_CLICK_THS 0x3a +#define ADDR_TIME_LIMIT 0x3b +#define ADDR_TIME_LATENCY 0x3c +#define ADDR_TIME_WINDOW 0x3d +#define ADDR_ACT_THS 0x3e +#define ADDR_ACT_DUR 0x3f + #define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) From 7d415b0c42465bffb79a3c69443e7bf85b59eb26 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 18:12:50 +1100 Subject: [PATCH 041/233] lsm303d: print more registers in "lsm303d regdump" --- src/drivers/lsm303d/lsm303d.cpp | 55 ++++++++++++++++++++++++++------- 1 file changed, 44 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index de227974ec..91f1fe0059 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1417,21 +1417,54 @@ LSM303D::print_registers() uint8_t reg; const char *name; } regmap[] = { - { ADDR_WHO_AM_I, "WHO_AM_I" }, - { ADDR_STATUS_A, "STATUS_A" }, - { ADDR_STATUS_M, "STATUS_M" }, - { ADDR_CTRL_REG0, "CTRL_REG0" }, - { ADDR_CTRL_REG1, "CTRL_REG1" }, - { ADDR_CTRL_REG2, "CTRL_REG2" }, - { ADDR_CTRL_REG3, "CTRL_REG3" }, - { ADDR_CTRL_REG4, "CTRL_REG4" }, - { ADDR_CTRL_REG5, "CTRL_REG5" }, - { ADDR_CTRL_REG6, "CTRL_REG6" }, - { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_OUT_TEMP_L, "TEMP_L" }, + { ADDR_OUT_TEMP_H, "TEMP_H" }, + { ADDR_INT_CTRL_M, "INT_CTRL_M" }, + { ADDR_INT_SRC_M, "INT_SRC_M" }, + { ADDR_REFERENCE_X, "REFERENCE_X" }, + { ADDR_REFERENCE_Y, "REFERENCE_Y" }, + { ADDR_REFERENCE_Z, "REFERENCE_Z" }, + { ADDR_OUT_X_L_A, "ACCEL_XL" }, + { ADDR_OUT_X_H_A, "ACCEL_XH" }, + { ADDR_OUT_Y_L_A, "ACCEL_YL" }, + { ADDR_OUT_Y_H_A, "ACCEL_YH" }, + { ADDR_OUT_Z_L_A, "ACCEL_ZL" }, + { ADDR_OUT_Z_H_A, "ACCEL_ZH" }, + { ADDR_FIFO_CTRL, "FIFO_CTRL" }, + { ADDR_FIFO_SRC, "FIFO_SRC" }, + { ADDR_IG_CFG1, "IG_CFG1" }, + { ADDR_IG_SRC1, "IG_SRC1" }, + { ADDR_IG_THS1, "IG_THS1" }, + { ADDR_IG_DUR1, "IG_DUR1" }, + { ADDR_IG_CFG2, "IG_CFG2" }, + { ADDR_IG_SRC2, "IG_SRC2" }, + { ADDR_IG_THS2, "IG_THS2" }, + { ADDR_IG_DUR2, "IG_DUR2" }, + { ADDR_CLICK_CFG, "CLICK_CFG" }, + { ADDR_CLICK_SRC, "CLICK_SRC" }, + { ADDR_CLICK_THS, "CLICK_THS" }, + { ADDR_TIME_LIMIT, "TIME_LIMIT" }, + { ADDR_TIME_LATENCY,"TIME_LATENCY" }, + { ADDR_TIME_WINDOW, "TIME_WINDOW" }, + { ADDR_ACT_THS, "ACT_THS" }, + { ADDR_ACT_DUR, "ACT_DUR" } }; for (uint8_t i=0; i Date: Wed, 30 Oct 2013 09:45:58 +1100 Subject: [PATCH 042/233] SPI: added set_frequency() API this allows the bus speed to be changed on the fly by device drivers. This is needed for the MPU6000 --- src/drivers/device/spi.cpp | 6 ++++++ src/drivers/device/spi.h | 11 +++++++++++ 2 files changed, 17 insertions(+) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fa6b78d640..fed6c312c1 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) return OK; } +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9103dca2ea..299575dc5c 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Set the SPI bus frequency + * This is used to change frequency on the fly. Some sensors + * (such as the MPU6000) need a lower frequency for setup + * registers and can handle higher frequency for sensor + * value registers + * + * @param frequency Frequency to set (Hz) + */ + void set_frequency(uint32_t frequency); + /** * Locking modes supported by the driver. */ From af47a3d795c01efbaabd60d6a15d48337187d35b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:46:52 +1100 Subject: [PATCH 043/233] mpu6000: change bus speed based on registers being accessed this ensures we follow the datasheet requirement of 1MHz for general registers and up to 20MHz for sensor and int status registers --- src/drivers/mpu6000/mpu6000.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 70359110ea..6bfa583fb0 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -161,6 +161,14 @@ #define MPU6000_ONE_G 9.80665f +/* + the MPU6000 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define MPU6000_LOW_BUS_SPEED 1000*1000 +#define MPU6000_HIGH_BUS_SPEED 10*1000*1000 + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -351,7 +359,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), + SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -991,6 +999,9 @@ MPU6000::read_reg(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; @@ -1003,6 +1014,9 @@ MPU6000::read_reg16(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; @@ -1016,6 +1030,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, nullptr, sizeof(cmd)); } @@ -1139,6 +1156,10 @@ MPU6000::measure() * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + // sensor transfer at high clock speed + set_frequency(MPU6000_HIGH_BUS_SPEED); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; From d0507296c0ce2f3614462a10f7b72d2d9fa5d8f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 15:27:48 +1100 Subject: [PATCH 044/233] px4io: moved blue heartbeat LED to main loop this allows us to tell if the main loop is running by looking for a blinking blue LED --- src/modules/px4iofirmware/px4io.c | 13 +++++++++++++ src/modules/px4iofirmware/safety.c | 16 +--------------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index ff9eecd74f..cd9bd197ba 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -117,6 +117,13 @@ show_debug_messages(void) } } +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + int user_start(int argc, char *argv[]) { @@ -201,6 +208,7 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ @@ -216,6 +224,11 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); + if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 95335f038a..cdb54a80ad 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -77,7 +77,6 @@ static unsigned blink_counter = 0; static bool safety_button_pressed; static void safety_check_button(void *arg); -static void heartbeat_blink(void *arg); static void failsafe_blink(void *arg); void @@ -86,9 +85,6 @@ safety_init(void) /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); - /* arrange for the heartbeat handler to be called at 4Hz */ - hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); - /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -163,16 +159,6 @@ safety_check_button(void *arg) } } -static void -heartbeat_blink(void *arg) -{ - static bool heartbeat = false; - - /* XXX add flags here that need to be frobbed by various loops */ - - LED_BLUE(heartbeat = !heartbeat); -} - static void failsafe_blink(void *arg) { @@ -192,4 +178,4 @@ failsafe_blink(void *arg) } LED_AMBER(failsafe); -} \ No newline at end of file +} From b6665819830425f6ba6afaad853f7d73cb820705 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 11:12:08 +1100 Subject: [PATCH 045/233] lsm303d: fixed TEMP_H register define --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 91f1fe0059..10af611ed7 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; #define WHO_I_AM 0x49 #define ADDR_OUT_TEMP_L 0x05 -#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_OUT_TEMP_H 0x06 #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 From 3decf408c2d8a2e12f838bebfd77e1359e22bd3f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Nov 2013 20:56:15 +1100 Subject: [PATCH 046/233] FMUv2: added support for MPU6000 on v2.4 board --- src/drivers/boards/px4fmu-v2/board_config.h | 2 ++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ec8dde4993..534394274c 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -85,11 +85,13 @@ __BEGIN_DECLS #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 09838d02fe..2527e4c143 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, * required for some peripheral @@ -81,6 +82,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); #endif #ifdef CONFIG_STM32_SPI2 @@ -99,6 +101,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_ACCEL_MAG: @@ -106,6 +109,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_BARO: @@ -113,6 +117,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: From 3a597d1a1f2fc278482e3be8e1872d7e77d32e9e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:28:42 +1100 Subject: [PATCH 047/233] FMUv2: added define for MPU DRDY pin --- src/drivers/boards/px4fmu-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 534394274c..9f66d195d7 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -79,6 +79,7 @@ __BEGIN_DECLS #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) #define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) #define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) From 720f6ab313ab869b89a4767d202ec2e64ddb22e9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:02 +1100 Subject: [PATCH 048/233] FMUv2: set MPU6000 CS as initially de-selected --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index ae2a645f70..31ad60bd34 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\n"); From cb76f07d3153895e379f15f6ca388ef04c6384dc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:33 +1100 Subject: [PATCH 049/233] l3gd20: added I2C disable based on method from ST engineering support --- src/drivers/l3gd20/l3gd20.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 8f56748234..31e38fbd9a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -218,6 +218,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -574,6 +579,7 @@ L3GD20::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); @@ -699,9 +705,19 @@ L3GD20::stop() hrt_cancel(&_call); } +void +L3GD20::disable_i2c(void) +{ + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); +} + void L3GD20::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ @@ -753,6 +769,7 @@ L3GD20::measure() perf_begin(_sample_perf); /* fetch data from the sensor */ + memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); From 92141548319898b2b0db94957b8a9281c33b5c47 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:30:04 +1100 Subject: [PATCH 050/233] lsm303d: added I2C disable based on method from ST engineering support --- src/drivers/lsm303d/lsm303d.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 10af611ed7..889c77b2cf 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -300,6 +300,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -563,9 +568,25 @@ out: return ret; } +void +LSM303D::disable_i2c(void) +{ + uint8_t a = read_reg(0x02); + write_reg(0x02, (0x10 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xF7 & a)); + a = read_reg(0x15); + write_reg(0x15, (0x80 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xE7 & a)); +} + void LSM303D::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* enable accel*/ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; write_reg(ADDR_CTRL_REG1, _reg1_expected); From 6ba54e70359ad1134503b170d726dc6edf29234f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:31:28 +1100 Subject: [PATCH 051/233] lsm303d: cleanup logic traces by pre-zeroing all transfers --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 889c77b2cf..4e39d71bc2 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1299,6 +1299,7 @@ LSM303D::measure() perf_begin(_accel_sample_perf); /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); @@ -1376,6 +1377,7 @@ LSM303D::mag_measure() perf_begin(_mag_sample_perf); /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); From 19853f87a25f10c1a9ff22c5cfce2536fe4b1b4b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:31:51 +1100 Subject: [PATCH 052/233] FMUv2: change CS pins to 2MHz this gives cleaner traces --- src/drivers/boards/px4fmu-v2/board_config.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 9f66d195d7..586661b58f 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -82,11 +82,11 @@ __BEGIN_DECLS #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) -#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 From bdb462379ac01b1f4fa25154624193c153647789 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:33:32 +1100 Subject: [PATCH 053/233] FMUv2: don't config ADC pins that are now used for MPU6k CS and other uses --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 31ad60bd34..269ec238eb 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ From ba8399780ae162b61240bf12bd8b093c91cf2bfe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 Dec 2013 07:57:22 +1100 Subject: [PATCH 054/233] init.d: added 3dr_skywalker airframe config params not tuned yet, but servos in the right direction --- ROMFS/px4fmu_common/init.d/102_3dr_skywalker | 89 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 ++ 2 files changed, 95 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/102_3dr_skywalker diff --git a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/102_3dr_skywalker new file mode 100644 index 0000000000..e5d21c3219 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/102_3dr_skywalker @@ -0,0 +1,89 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +pwm disarmed -c 3 -p 1056 + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index d8b5cb6087..f1df990487 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -313,6 +313,12 @@ then sh /etc/init.d/101_hk_bixler set MODE custom fi + + if param compare SYS_AUTOSTART 102 + then + sh /etc/init.d/102_3dr_skywalker + set MODE custom + fi # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] From 193692cc0df0c3a4b9905fc18cb04321095b6984 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 1 Dec 2013 22:44:36 +0100 Subject: [PATCH 055/233] Hex Startup: Set rate of all 8 PWM outputs (6 are not possible because rate can only be changed for channel groups --- ROMFS/px4fmu_common/init.d/12-13_hex | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex index f83f6cfd05..a7578bcaf8 100644 --- a/ROMFS/px4fmu_common/init.d/12-13_hex +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -78,7 +78,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix # # Set PWM output frequency to 400 Hz # -pwm rate -c 123456 -r 400 +pwm rate -a -r 400 # # Set disarmed, min and max PWM signals From dcfd5bdbe7d999db70e3d4e3e08320e03cc3840a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:07:36 +0100 Subject: [PATCH 056/233] Python uploader: Ignore exceptions when sending reboot tries --- Tools/px_uploader.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index a2d7d371df..cce5e5e54a 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -370,14 +370,17 @@ class uploader(object): self.port.close() def send_reboot(self): - # try reboot via NSH first - self.__send(uploader.NSH_INIT) - self.__send(uploader.NSH_REBOOT_BL) - self.__send(uploader.NSH_INIT) - self.__send(uploader.NSH_REBOOT) - # then try MAVLINK command - self.__send(uploader.MAVLINK_REBOOT_ID1) - self.__send(uploader.MAVLINK_REBOOT_ID0) + try: + # try reboot via NSH first + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT_BL) + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT) + # then try MAVLINK command + self.__send(uploader.MAVLINK_REBOOT_ID1) + self.__send(uploader.MAVLINK_REBOOT_ID0) + except: + return From b2119839bd801a3b63ae85b4c4acdb4f227343ff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Nov 2013 08:11:58 +1100 Subject: [PATCH 057/233] lsm303d: init filter to 773 Hz --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4e39d71bc2..47109b67dc 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -599,7 +599,7 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); From edc5b684990958c91fbc962cd3ba656645222feb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 16:12:25 +1100 Subject: [PATCH 058/233] l3gd20: use highest possible on-chip filter bandwidth this allows the software filter to do its job properly --- src/drivers/l3gd20/l3gd20.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 31e38fbd9a..103b26ac58 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -92,9 +92,12 @@ static const int ERROR = -1; #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -659,16 +662,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_25HZ; + bits |= RATE_190HZ_LP_70HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_20HZ; + bits |= RATE_380HZ_LP_100HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_30HZ; - + bits |= RATE_760HZ_LP_100HZ; } else { return -EINVAL; } @@ -732,7 +734,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(L3GD20_DEFAULT_RATE); + set_samplerate(0); // 760Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); From 881cf61553f1acce6054db635e0d1af11476eb3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Dec 2013 07:57:23 +0100 Subject: [PATCH 059/233] Added IOCTL and command for sensor rail reset (does not yet re-initialize sensor drivers) --- src/drivers/boards/px4fmu-v2/board_config.h | 17 +++++ src/drivers/px4fmu/fmu.cpp | 78 ++++++++++++++++++++- 2 files changed, 94 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 586661b58f..7ab63953f8 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -81,6 +81,23 @@ __BEGIN_DECLS #define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +/* Data ready pins off */ +#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) +#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) +#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) +#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6) +#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7) + +/* SPI1 chip selects off */ +#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) + /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e98..9433ab59fa 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1052,6 +1052,71 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) return count * 2; } +void +PX4FMU::sensor_reset(int ms) +{ + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); + stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + + stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + stm32_configgpio(GPIO_GYRO_DRDY_OFF); + stm32_configgpio(GPIO_MAG_DRDY_OFF); + stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + + stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + + /* set the sensor rail off */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 000); + + /* re-enable power */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); +#endif +} + void PX4FMU::gpio_reset(void) { @@ -1154,6 +1219,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) gpio_reset(); break; + case SENSOR_RESET: + sensor_reset(); + break; + case GPIO_SET_OUTPUT: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: @@ -1489,11 +1558,18 @@ fmu_main(int argc, char *argv[]) if (!strcmp(verb, "fake")) fake(argc - 1, argv + 1); + if (!strcmp(verb, "sensor_reset")) + if (argc > 2) { + sensor_reset(strtol(argv[2], 0, 0)); + } else { + sensor_reset(0); + } + fprintf(stderr, "FMU: unrecognised command, try:\n"); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fprintf(stderr, " mode_gpio, mode_pwm, test\n"); + fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); #endif exit(1); } From acc3cc087f72609efa9d3450f640e2980fe1eb86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Dec 2013 08:17:35 +0100 Subject: [PATCH 060/233] Added sensor rail reset IOCTL and command (fmu sensor_reset 10 resets for 10 ms) --- src/drivers/drv_gpio.h | 4 +- src/drivers/px4fmu/fmu.cpp | 456 ++++++++++++++++++++++--------------- 2 files changed, 280 insertions(+), 180 deletions(-) diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 37af26d522..f60964c2b1 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -46,7 +46,7 @@ /* * PX4FMU GPIO numbers. * - * For shared pins, alternate function 1 selects the non-GPIO mode + * For shared pins, alternate function 1 selects the non-GPIO mode * (USART2, CAN2, etc.) */ # define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */ @@ -144,4 +144,6 @@ /** read all the GPIOs and return their values in *(uint32_t *)arg */ #define GPIO_GET GPIOC(12) +#define GPIO_SENSOR_RAIL_RESET GPIOC(13) + #endif /* _DRV_GPIO_H */ \ No newline at end of file diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9433ab59fa..d37c260f07 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -164,6 +164,7 @@ private: static const unsigned _ngpio; void gpio_reset(void); + void sensor_reset(int ms); void gpio_set_function(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function); uint32_t gpio_read(void); @@ -226,10 +227,10 @@ PX4FMU::PX4FMU() : _armed(false), _pwm_on(false), _mixers(nullptr), - _failsafe_pwm({0}), - _disarmed_pwm({0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _failsafe_pwm( {0}), + _disarmed_pwm( {0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -293,11 +294,11 @@ PX4FMU::init() /* start the IO interface task */ _task = task_spawn_cmd("fmuservo", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&PX4FMU::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&PX4FMU::task_main_trampoline, + nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -326,7 +327,7 @@ PX4FMU::set_mode(Mode mode) switch (mode) { case MODE_2PWM: // v1 multi-port with flow control lines as PWM debug("MODE_2PWM"); - + /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -340,7 +341,7 @@ PX4FMU::set_mode(Mode mode) case MODE_4PWM: // v1 multi-port as 4 PWM outs debug("MODE_4PWM"); - + /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -354,7 +355,7 @@ PX4FMU::set_mode(Mode mode) case MODE_6PWM: // v2 PWMs as 6 PWM outs debug("MODE_6PWM"); - + /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -396,6 +397,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate // get the channel mask for this rate group uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) continue; @@ -409,6 +411,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate // not a legal map, bail return -EINVAL; } + } else { // set it - errors here are unexpected if (alt != 0) { @@ -416,6 +419,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate warn("rate group set alt failed"); return -EINVAL; } + } else { if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { warn("rate group set default failed"); @@ -425,6 +429,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate } } } + _pwm_alt_rate_channels = rate_map; _pwm_default_rate = default_rate; _pwm_alt_rate = alt_rate; @@ -471,7 +476,7 @@ PX4FMU::task_main() memset(&controls_effective, 0, sizeof(controls_effective)); /* advertise the effective control inputs */ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); + &controls_effective); pollfd fds[2]; fds[0].fd = _t_actuators; @@ -503,6 +508,7 @@ PX4FMU::task_main() * We always mix at max rate; some channels may update slower. */ unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + if (_current_update_rate != max_rate) { _current_update_rate = max_rate; int update_rate_in_ms = int(1000 / _current_update_rate); @@ -511,6 +517,7 @@ PX4FMU::task_main() if (update_rate_in_ms < 2) { update_rate_in_ms = 2; } + /* reject slower than 10 Hz updates */ if (update_rate_in_ms > 100) { update_rate_in_ms = 100; @@ -532,6 +539,7 @@ PX4FMU::task_main() log("poll error %d", errno); usleep(1000000); continue; + } else if (ret == 0) { /* timeout: no control data, switch to failsafe values */ // warnx("no PWM: failsafe"); @@ -553,12 +561,15 @@ PX4FMU::task_main() case MODE_2PWM: num_outputs = 2; break; + case MODE_4PWM: num_outputs = 4; break; + case MODE_6PWM: num_outputs = 6; break; + default: num_outputs = 0; break; @@ -572,9 +583,9 @@ PX4FMU::task_main() for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -592,6 +603,7 @@ PX4FMU::task_main() for (unsigned i = 0; i < num_outputs; i++) { controls_effective.control_effective[i] = (float)pwm_limited[i]; } + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); /* output to the servos */ @@ -613,11 +625,13 @@ PX4FMU::task_main() /* update the armed status and check that we're not locked down */ bool set_armed = aa.armed && !aa.lockdown; + if (_armed != set_armed) _armed = set_armed; /* update PWM status if armed or if disarmed PWM values are set */ bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { _pwm_on = pwm_on; up_pwm_servo_arm(pwm_on); @@ -626,25 +640,31 @@ PX4FMU::task_main() } #ifdef HRT_PPM_CHANNEL + // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { rc_in.channel_count = RC_INPUT_MAX_CHANNELS; } - for (uint8_t i=0; ichannel_count > _max_actuators) { - ret = -EINVAL; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _failsafe_pwm[i] = PWM_HIGHEST_MAX; + + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _failsafe_pwm[i] = PWM_LOWEST_MIN; + + } else { + _failsafe_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_failsafe_set = 0; + + for (unsigned i = 0; i < _max_actuators; i++) { + if (_failsafe_pwm[i] > 0) + _num_failsafe_set++; + } + break; } - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _failsafe_pwm[i] = PWM_HIGHEST_MAX; - } else if (pwm->values[i] < PWM_LOWEST_MIN) { - _failsafe_pwm[i] = PWM_LOWEST_MIN; - } else { - _failsafe_pwm[i] = pwm->values[i]; - } - } - /* - * update the counter - * this is needed to decide if disarmed PWM output should be turned on or not - */ - _num_failsafe_set = 0; - for (unsigned i = 0; i < _max_actuators; i++) { - if (_failsafe_pwm[i] > 0) - _num_failsafe_set++; - } - break; - } case PWM_SERVO_GET_FAILSAFE_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values*)arg; - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _failsafe_pwm[i]; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _failsafe_pwm[i]; + } + + pwm->channel_count = _max_actuators; + break; } - pwm->channel_count = _max_actuators; - break; - } case PWM_SERVO_SET_DISARMED_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values*)arg; - /* discard if too many values are sent */ - if (pwm->channel_count > _max_actuators) { - ret = -EINVAL; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _disarmed_pwm[i] = PWM_HIGHEST_MAX; + + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _disarmed_pwm[i] = PWM_LOWEST_MIN; + + } else { + _disarmed_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_disarmed_set = 0; + + for (unsigned i = 0; i < _max_actuators; i++) { + if (_disarmed_pwm[i] > 0) + _num_disarmed_set++; + } + break; } - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _disarmed_pwm[i] = PWM_HIGHEST_MAX; - } else if (pwm->values[i] < PWM_LOWEST_MIN) { - _disarmed_pwm[i] = PWM_LOWEST_MIN; - } else { - _disarmed_pwm[i] = pwm->values[i]; - } - } - /* - * update the counter - * this is needed to decide if disarmed PWM output should be turned on or not - */ - _num_disarmed_set = 0; - for (unsigned i = 0; i < _max_actuators; i++) { - if (_disarmed_pwm[i] > 0) - _num_disarmed_set++; - } - break; - } case PWM_SERVO_GET_DISARMED_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values*)arg; - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _disarmed_pwm[i]; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _disarmed_pwm[i]; + } + + pwm->channel_count = _max_actuators; + break; } - pwm->channel_count = _max_actuators; - break; - } case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - /* discard if too many values are sent */ - if (pwm->channel_count > _max_actuators) { - ret = -EINVAL; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MIN) { + _min_pwm[i] = PWM_HIGHEST_MIN; + + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _min_pwm[i] = PWM_LOWEST_MIN; + + } else { + _min_pwm[i] = pwm->values[i]; + } + } + break; } - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MIN) { - _min_pwm[i] = PWM_HIGHEST_MIN; - } else if (pwm->values[i] < PWM_LOWEST_MIN) { - _min_pwm[i] = PWM_LOWEST_MIN; - } else { - _min_pwm[i] = pwm->values[i]; - } - } - break; - } + case PWM_SERVO_GET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values*)arg; - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _min_pwm[i]; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _min_pwm[i]; + } + + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; } - pwm->channel_count = _max_actuators; - arg = (unsigned long)&pwm; - break; - } case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - /* discard if too many values are sent */ - if (pwm->channel_count > _max_actuators) { - ret = -EINVAL; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] < PWM_LOWEST_MAX) { + _max_pwm[i] = PWM_LOWEST_MAX; + + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _max_pwm[i] = PWM_HIGHEST_MAX; + + } else { + _max_pwm[i] = pwm->values[i]; + } + } + break; } - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] < PWM_LOWEST_MAX) { - _max_pwm[i] = PWM_LOWEST_MAX; - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _max_pwm[i] = PWM_HIGHEST_MAX; - } else { - _max_pwm[i] = pwm->values[i]; - } - } - break; - } + case PWM_SERVO_GET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values*)arg; - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _max_pwm[i]; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _max_pwm[i]; + } + + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; } - pwm->channel_count = _max_actuators; - arg = (unsigned long)&pwm; - break; - } case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): @@ -910,6 +964,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0): if (arg <= 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); + } else { ret = -EINVAL; } @@ -946,18 +1001,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; - case PWM_SERVO_GET_COUNT: + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { case MODE_6PWM: *(unsigned *)arg = 6; break; + case MODE_4PWM: *(unsigned *)arg = 4; break; + case MODE_2PWM: *(unsigned *)arg = 2; break; + default: ret = -EINVAL; break; @@ -1015,6 +1073,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } + break; } @@ -1049,55 +1108,58 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) for (uint8_t i = 0; i < count; i++) { up_pwm_servo_set(i, values[i]); } + return count * 2; } void PX4FMU::sensor_reset(int ms) { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + if (ms < 1) { ms = 1; } /* disable SPI bus */ - stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); - stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); + stm32_configgpio(GPIO_SPI_CS_BARO_OFF); - stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); - stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - stm32_configgpio(GPIO_GYRO_DRDY_OFF); - stm32_configgpio(GPIO_MAG_DRDY_OFF); - stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_GYRO_DRDY_OFF); + stm32_configgpio(GPIO_MAG_DRDY_OFF); + stm32_configgpio(GPIO_ACCEL_DRDY_OFF); - stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); - /* set the sensor rail off */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + /* set the sensor rail off */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - /* wait for the sensor rail to reach GND */ - usleep(ms * 000); + /* wait for the sensor rail to reach GND */ + usleep(ms * 000); /* re-enable power */ /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); /* reconfigure the SPI pins */ #ifdef CONFIG_STM32_SPI1 @@ -1115,8 +1177,10 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); #endif +#endif } + void PX4FMU::gpio_reset(void) { @@ -1127,6 +1191,7 @@ PX4FMU::gpio_reset(void) for (unsigned i = 0; i < _ngpio; i++) { if (_gpio_tab[i].input != 0) { stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { stm32_configgpio(_gpio_tab[i].output); } @@ -1143,6 +1208,7 @@ void PX4FMU::gpio_set_function(uint32_t gpios, int function) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. @@ -1154,6 +1220,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) if (GPIO_SET_OUTPUT == function) stm32_gpiowrite(GPIO_GPIO_DIR, 1); } + #endif /* configure selected GPIOs as required */ @@ -1178,9 +1245,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) } #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* flip buffer to input mode if required */ if ((GPIO_SET_INPUT == function) && (gpios & 3)) stm32_gpiowrite(GPIO_GPIO_DIR, 0); + #endif } @@ -1219,8 +1288,8 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) gpio_reset(); break; - case SENSOR_RESET: - sensor_reset(); + case GPIO_SENSOR_RAIL_RESET: + sensor_reset(20); break; case GPIO_SET_OUTPUT: @@ -1296,8 +1365,9 @@ fmu_new_mode(PortMode new_mode) #endif break; - /* mixed modes supported on v1 board only */ + /* mixed modes supported on v1 board only */ #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + case PORT_FULL_SERIAL: /* set all multi-GPIOs to serial mode */ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; @@ -1320,6 +1390,7 @@ fmu_new_mode(PortMode new_mode) servo_mode = PX4FMU::MODE_2PWM; break; #endif + default: return -1; } @@ -1373,15 +1444,31 @@ fmu_stop(void) return ret; } +void +sensor_reset(int ms) +{ + int fd; + int ret; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); + + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) + err(1, "servo arm failed"); + +} + void test(void) { int fd; - unsigned servo_count = 0; + unsigned servo_count = 0; unsigned pwm_value = 1000; int direction = 1; int ret; - + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) @@ -1389,9 +1476,9 @@ test(void) if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { - err(1, "Unable to get servo count\n"); - } + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { + err(1, "Unable to get servo count\n"); + } warnx("Testing %u servos", (unsigned)servo_count); @@ -1404,32 +1491,38 @@ test(void) for (;;) { /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) servos[i] = pwm_value; - if (direction == 1) { - // use ioctl interface for one direction - for (unsigned i=0; i < servo_count; i++) { - if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { - err(1, "servo %u set failed", i); - } - } - } else { - // and use write interface for the other direction - ret = write(fd, servos, sizeof(servos)); - if (ret != (int)sizeof(servos)) - err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); - } + if (direction == 1) { + // use ioctl interface for one direction + for (unsigned i = 0; i < servo_count; i++) { + if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { + err(1, "servo %u set failed", i); + } + } + + } else { + // and use write interface for the other direction + ret = write(fd, servos, sizeof(servos)); + + if (ret != (int)sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } if (direction > 0) { if (pwm_value < 2000) { pwm_value++; + } else { direction = -1; } + } else { if (pwm_value > 1000) { pwm_value--; + } else { direction = 1; } @@ -1441,6 +1534,7 @@ test(void) if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) err(1, "error reading PWM servo %d", i); + if (value != servos[i]) errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } @@ -1448,12 +1542,14 @@ test(void) /* Check if user wants to quit */ char c; ret = poll(&fds, 1, 0); + if (ret > 0) { read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); - break; + break; } } } @@ -1526,6 +1622,7 @@ fmu_main(int argc, char *argv[]) new_mode = PORT_FULL_PWM; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + } else if (!strcmp(verb, "mode_serial")) { new_mode = PORT_FULL_SERIAL; @@ -1561,6 +1658,7 @@ fmu_main(int argc, char *argv[]) if (!strcmp(verb, "sensor_reset")) if (argc > 2) { sensor_reset(strtol(argv[2], 0, 0)); + } else { sensor_reset(0); } From 012adc9e33bb9a92030174936546e67383b91a7a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Dec 2013 09:25:07 +0100 Subject: [PATCH 061/233] Minor fixes to bus reset --- src/drivers/ms5611/ms5611.cpp | 7 +++++-- src/drivers/px4fmu/fmu.cpp | 14 ++++++++++---- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 938786d3f1..87788824a9 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -420,8 +420,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return _reports->size(); case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + /* + * Since we are initialized, we do not need to do anything, since the + * PROM is correctly read and the part does not need to be configured. + */ + return OK; case BAROIOCSMSLPRESSURE: diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d37c260f07..aab532514f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1151,7 +1151,8 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); /* wait for the sensor rail to reach GND */ - usleep(ms * 000); + usleep(ms * 1000); + warnx("reset done, %d ms", ms); /* re-enable power */ @@ -1289,7 +1290,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) break; case GPIO_SENSOR_RAIL_RESET: - sensor_reset(20); + sensor_reset(arg); break; case GPIO_SET_OUTPUT: @@ -1655,13 +1656,18 @@ fmu_main(int argc, char *argv[]) if (!strcmp(verb, "fake")) fake(argc - 1, argv + 1); - if (!strcmp(verb, "sensor_reset")) + if (!strcmp(verb, "sensor_reset")) { if (argc > 2) { - sensor_reset(strtol(argv[2], 0, 0)); + int reset_time = strtol(argv[2], 0, 0); + sensor_reset(reset_time); } else { sensor_reset(0); + warnx("resettet default time"); } + exit(0); + } + fprintf(stderr, "FMU: unrecognised command, try:\n"); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) From 264ef47197432d2cc1372cabf93c3bd7a52df0aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Dec 2013 05:02:00 +0100 Subject: [PATCH 062/233] PPM loopback test --- src/systemcmds/tests/test_ppm_loopback.c | 108 ++++++++++++++--------- 1 file changed, 66 insertions(+), 42 deletions(-) diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index 6d4e45c4ca..b9041b0139 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -51,6 +51,7 @@ #include #include #include +#include #include #include "tests.h" @@ -61,6 +62,8 @@ int test_ppm_loopback(int argc, char *argv[]) { + int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + int servo_fd, result; servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; @@ -71,12 +74,6 @@ int test_ppm_loopback(int argc, char *argv[]) printf("failed opening /dev/pwm_servo\n"); } - result = read(servo_fd, &data, sizeof(data)); - - if (result != sizeof(data)) { - printf("failed bulk-reading channel values\n"); - } - printf("Servo readback, pairs of values should match defaults\n"); unsigned servo_count; @@ -93,62 +90,89 @@ int test_ppm_loopback(int argc, char *argv[]) printf("failed reading channel %u\n", i); } - printf("%u: %u %u\n", i, pos, data[i]); + //printf("%u: %u %u\n", i, pos, data[i]); } - /* tell safety that its ok to disable it with the switch */ - result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); - if (result != OK) - warnx("FAIL: PWM_SERVO_SET_ARM_OK"); - /* tell output device that the system is armed (it will output values if safety is off) */ - result = ioctl(servo_fd, PWM_SERVO_ARM, 0); - if (result != OK) - warnx("FAIL: PWM_SERVO_ARM"); + // /* tell safety that its ok to disable it with the switch */ + // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); + // if (result != OK) + // warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + // tell output device that the system is armed (it will output values if safety is off) + // result = ioctl(servo_fd, PWM_SERVO_ARM, 0); + // if (result != OK) + // warnx("FAIL: PWM_SERVO_ARM"); int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; - printf("Advancing channel 0 to 1100\n"); - result = ioctl(servo_fd, PWM_SERVO_SET(0), 1100); - printf("Advancing channel 1 to 1900\n"); - result = ioctl(servo_fd, PWM_SERVO_SET(1), 1900); - printf("Advancing channel 2 to 1200\n"); - result = ioctl(servo_fd, PWM_SERVO_SET(2), 1200); + // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + + // if (result) { + // (void)close(servo_fd); + // return ERROR; + // } else { + // warnx("channel %d set to %d", i, pwm_values[i]); + // } + // } + + warnx("servo count: %d", servo_count); + + struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0}; for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); - if (result) { - (void)close(servo_fd); - return ERROR; - } + pwm_out.values[i] = pwm_values[i]; + //warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]); + pwm_out.channel_count++; } + result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out); + /* give driver 10 ms to propagate */ - usleep(10000); + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + usleep(100000); /* open PPM input and expect values close to the output values */ - int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - struct rc_input_values rc; - result = read(ppm_fd, &rc, sizeof(rc)); + if (rc_updated) { - if (result != sizeof(rc)) { - warnx("Error reading RC output"); - (void)close(servo_fd); - (void)close(ppm_fd); + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + // int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); + + + + // struct rc_input_values rc; + // result = read(ppm_fd, &rc, sizeof(rc)); + + // if (result != sizeof(rc)) { + // warnx("Error reading RC output"); + // (void)close(servo_fd); + // (void)close(ppm_fd); + // return ERROR; + // } + + /* go and check values */ + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) { + warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]); + (void)close(servo_fd); + return ERROR; + } + } + } else { + warnx("failed reading RC input data"); return ERROR; } - /* go and check values */ - for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - result = ioctl(servo_fd, PWM_SERVO_GET(i), pwm_values[i]); - if (result) { - (void)close(servo_fd); - return ERROR; - } - } + warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!"); return 0; } From 7becbcdbd59c0842ff44e682df0f13fd067def10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:34:25 +0100 Subject: [PATCH 063/233] Made all usual suspects default to their custom names and only register the default name if its not already taken by someone else --- src/drivers/hmc5883/hmc5883.cpp | 7 ++++--- src/drivers/l3gd20/l3gd20.cpp | 7 +++++++ src/drivers/lsm303d/lsm303d.cpp | 33 ++++++++++++++++++++++++--------- src/drivers/mpu6000/mpu6000.cpp | 33 +++++++++++++++++++++++++-------- 4 files changed, 60 insertions(+), 20 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5f0ce4ff86..22ad301ff3 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,6 +77,7 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 +#define HMC5883L_DEVICE_PATH "/dev/hmc5883" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -315,7 +316,7 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : - I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1256,7 +1257,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -1288,7 +1289,7 @@ test() ssize_t sz; int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 103b26ac58..d816b1a592 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -360,6 +360,13 @@ L3GD20::init() if (_reports == nullptr) goto out; + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + /* advertise sensor topic */ struct gyro_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 47109b67dc..229b052a98 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -78,7 +78,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) - +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" +#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -551,6 +552,20 @@ LSM303D::init() reset(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); + + if (mag_ret == OK) { + log("default mag device"); + } + /* advertise mag topic */ struct mag_report zero_mag_report; memset(&zero_mag_report, 0, sizeof(zero_mag_report)); @@ -1491,7 +1506,7 @@ LSM303D::print_registers() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), + CDev("LSM303D_mag", "/dev/lsm303d_mag"), _parent(parent) { } @@ -1556,7 +1571,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1567,7 +1582,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1575,7 +1590,7 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); /* don't fail if open cannot be opened */ if (0 <= fd_mag) { @@ -1610,10 +1625,10 @@ test() int ret; /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); @@ -1639,10 +1654,10 @@ test() struct mag_report m_report; /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6bfa583fb0..ce00107521 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -75,6 +75,9 @@ #define DIR_READ 0x80 #define DIR_WRITE 0x00 +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" + // MPU 6000 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 @@ -359,7 +362,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -468,6 +471,20 @@ MPU6000::init() /* fetch an initial set of measurements for advertisement */ measure(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + if (gyro_ret != OK) { _gyro_topic = -1; } else { @@ -1290,7 +1307,7 @@ MPU6000::print_info() } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", GYRO_DEVICE_PATH), + CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent) { } @@ -1352,7 +1369,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1384,17 +1401,17 @@ test() ssize_t sz; /* get the driver */ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + MPU_DEVICE_PATH_ACCEL); /* get the driver */ - int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1452,7 +1469,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); From c72162cc5ae12e2e64f9a4fb86e205bab51a5b6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:44:29 +0100 Subject: [PATCH 064/233] Add also default descriptor for alternate sensors --- src/drivers/lsm303d/lsm303d.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 229b052a98..c9e4fe1f8f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -552,11 +552,21 @@ LSM303D::init() reset(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ + /* register the first instance as plain name, the 2nd as two and counting */ ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default accel device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* try to claim the generic accel node as well - it's OK if we fail at this */ @@ -564,6 +574,16 @@ LSM303D::init() if (mag_ret == OK) { log("default mag device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* advertise mag topic */ From 0ba507b640223e2bf45d3727cac1603bef215dde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Dec 2013 11:25:45 +0100 Subject: [PATCH 065/233] Added support for a total of four control groups to the IO driver and IO firmware. This allows to run auxiliary payload. Cleaned up defines for RC input channel counts, this needs another sweep to then finally allow up to 16 mapped channels and up to 20-24 RAW RC channels --- src/drivers/px4io/px4io.cpp | 151 +++++++++++++++++++------- src/modules/px4iofirmware/dsm.c | 2 +- src/modules/px4iofirmware/mixer.cpp | 4 +- src/modules/px4iofirmware/protocol.h | 54 +++++---- src/modules/px4iofirmware/px4io.h | 6 +- src/modules/px4iofirmware/registers.c | 18 +-- src/modules/px4iofirmware/sbus.c | 2 +- 7 files changed, 166 insertions(+), 71 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ef6ca04e97..702ec1c3ae 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -226,30 +226,33 @@ private: device::Device *_interface; // XXX - unsigned _hardware; ///< Hardware revision - unsigned _max_actuators; /// 100) _update_interval = 100; - orb_set_interval(_t_actuators, _update_interval); + orb_set_interval(_t_actuator_controls_0, _update_interval); + /* + * NOT changing the rate of groups 1-3 here, because only attitude + * really needs to run fast. + */ _update_interval = 0; } @@ -827,7 +847,10 @@ PX4IO::task_main() /* if we have new control data from the ORB, handle it */ if (fds[0].revents & POLLIN) { - io_set_control_state(); + + /* we're not nice to the lower-priority control groups and only check them + when the primary group updated (which is now). */ + io_set_control_groups(); } if (now >= poll_last + IO_POLL_INTERVAL) { @@ -871,7 +894,7 @@ PX4IO::task_main() orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); // Check for a DSM pairing command - if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) { + if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { dsm_bind_ioctl((int)cmd.param2); } } @@ -922,20 +945,74 @@ out: } int -PX4IO::io_set_control_state() +PX4IO::io_set_control_groups() +{ + bool attitude_ok = io_set_control_state(0); + + /* send auxiliary control groups */ + (void)io_set_control_state(0); + (void)io_set_control_state(1); + (void)io_set_control_state(2); + + return attitude_ok; +} + +int +PX4IO::io_set_control_state(unsigned group) { actuator_controls_s controls; ///< actuator outputs uint16_t regs[_max_actuators]; /* get controls */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &controls); + bool changed; + + switch (group) { + case 0: + { + orb_check(_t_actuator_controls_0, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls); + } + } + break; + case 1: + { + orb_check(_t_actuator_controls_1, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls); + } + } + break; + case 2: + { + orb_check(_t_actuator_controls_2, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls); + } + } + break; + case 3: + { + orb_check(_t_actuator_controls_3, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls); + } + } + break; + } + + if (!changed) + return -1; for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); + return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); } diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index fd3b720150..4d306d6d00 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) continue; /* ignore channels out of range */ - if (channel >= PX4IO_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) continue; /* update the decoded channel count */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 05897b4ce6..9bb93ce9f1 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -236,13 +236,13 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group != 0) + if (control_group > 3) return -1; switch (source) { case MIX_FMU: if (control_index < PX4IO_CONTROL_CHANNELS) { - control = REG_TO_FLOAT(r_page_controls[control_index]); + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } return -1; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 5e5396782b..832369f003 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -63,7 +63,7 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, * [2] denotes definitions specific to the PX4IOv2 board. */ @@ -76,6 +76,9 @@ #define PX4IO_PROTOCOL_VERSION 4 +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ @@ -87,6 +90,7 @@ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/ /* dynamic status page */ #define PX4IO_PAGE_STATUS 1 @@ -184,44 +188,54 @@ enum { /* DSM bind states */ dsm_bind_reinit_uart }; /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 52 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ -#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ -#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ -#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ -#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */ -#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */ -#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) -#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* Debug and test page - not used in normal operation */ -#define PX4IO_PAGE_TEST 127 -#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ /* PWM minimum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM maximum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM disarmed values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4fea0288c4..61eacd6023 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -53,7 +53,9 @@ */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels +#define PX4IO_CONTROL_GROUPS 2 +#define PX4IO_RC_INPUT_CHANNELS 8 // XXX this should be 18 channels +#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ /* * Debug logging @@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit; #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) + /* * Mixer */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 86a40bc228..0533f1d767 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -68,7 +68,7 @@ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS, [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -112,7 +112,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -122,7 +122,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -172,7 +172,7 @@ volatile uint16_t r_page_setup[] = * * Control values from the FMU. */ -volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; +volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS]; /* * PAGE 102 does not have a buffer. @@ -183,7 +183,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -235,7 +235,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_CONTROLS: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ r_page_controls[offset] = *values; @@ -525,7 +525,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= PX4IO_CONTROL_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -573,7 +573,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index c523df6caf..68a52c413a 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -239,7 +239,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint } /* decode switch channels if data fields are wide enough */ - if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ From cbde8d27f8a18eeb14038521115fd5bd2f97e29a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 20:14:32 +0100 Subject: [PATCH 066/233] fix small copy paste error in px4io driver --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 702ec1c3ae..f313a98f2b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -950,9 +950,9 @@ PX4IO::io_set_control_groups() bool attitude_ok = io_set_control_state(0); /* send auxiliary control groups */ - (void)io_set_control_state(0); (void)io_set_control_state(1); (void)io_set_control_state(2); + (void)io_set_control_state(3); return attitude_ok; } From 5e273bf225ac5ed57c10093296a00ee190cfbf7d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 21:34:31 +0100 Subject: [PATCH 067/233] px4iofirmware: in manual mode: ignore control indices which are not controlled by the rmeote control --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9bb93ce9f1..87844ca70a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -248,7 +248,7 @@ mixer_callback(uintptr_t handle, return -1; case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } From 3f0f34a4c786e7b8baccf57b2c22eddd6ee7c97f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:55:10 +1100 Subject: [PATCH 068/233] ms5611: give cleaner SPI traces this makes logic traces cleaner by zeroing extra bytes written --- src/drivers/ms5611/ms5611_spi.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e547c913ba..85504b0bdd 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -167,10 +167,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[4]; + uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 }; /* read the most recent measurement */ - buf[0] = 0 | DIR_WRITE; int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { @@ -241,18 +240,21 @@ MS5611_SPI::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + if (ret != OK) { + debug("crc failed"); + } + return ret; } uint16_t MS5611_SPI::_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; _transfer(cmd, cmd, sizeof(cmd)); From a52e70ca93a78edeb8d14ae95f881e0415e13760 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:54:58 +1100 Subject: [PATCH 069/233] ms5611: removed unused variable --- src/drivers/ms5611/ms5611_spi.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 85504b0bdd..65c23ae574 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,7 +134,6 @@ int MS5611_SPI::init() { int ret; - irqstate_t flags; ret = SPI::init(); if (ret != OK) { From b0bb5a34508c72efbbfc2ec622a2cd8a95e9df1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:53:25 +1100 Subject: [PATCH 070/233] ms5611: change bus speed to 5MHz this gives 5MHz SPI bus speed (by asking for 6MHz due to timer granularity). Tests with a logic analyser show that the ms5611 is actually more reliable at 5MHz than lower speeds --- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 65c23ae574..e9dff5a8bc 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000), _prom(prom_buf) { } From 476070510eca3c1eb8c485b5f2d04061dfb24f88 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 22:54:02 +1100 Subject: [PATCH 071/233] lsm303d/l3gd20: change filters to 50Hz analog on-chip filters after discussion with Leonard these analog on-chip filters should be at 50Hz --- src/drivers/l3gd20/l3gd20.cpp | 11 ++++++++--- src/drivers/lsm303d/lsm303d.cpp | 7 ++++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 103b26ac58..17b53bfa96 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -93,10 +93,15 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 @@ -662,15 +667,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_70HZ; + bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_100HZ; + bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_100HZ; + bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 47109b67dc..356569f998 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -599,7 +599,12 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz + + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); From 86ec1c37fa29ba4ffc1d54a0c438de1cd536f51c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Dec 2013 09:06:53 +1100 Subject: [PATCH 072/233] l3gd20: added retries to disable_i2c() --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 17b53bfa96..176ef648bb 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -715,8 +715,16 @@ L3GD20::stop() void L3GD20::disable_i2c(void) { - uint8_t a = read_reg(0x05); - write_reg(0x05, (0x20 | a)); + uint8_t retries = 10; + while (retries--) { + // add retries + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); + if (read_reg(0x05) == (a | 0x20)) { + return; + } + } + debug("FAILED TO DISABLE I2C"); } void From 53f2dc8296d550df8933663465cf163ae523084a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:00 +1100 Subject: [PATCH 073/233] drv_hrt: added hrt_call_init() and hrt_call_delay() APIs hrt_call_init() can be used to initialise (zero) a hrt_call structure to ensure safe usage. The hrt_call_every() interface calls this automatically. hrt_call_delay() can be used to delay a current callout by the given number of microseconds --- src/drivers/drv_hrt.h | 14 ++++++++++++++ src/drivers/stm32/drv_hrt.c | 19 ++++++++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8a99eeca79..d130d68b34 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry); */ __EXPORT extern void hrt_cancel(struct hrt_call *entry); +/* + * initialise a hrt_call structure + */ +__EXPORT extern void hrt_call_init(struct hrt_call *entry); + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); + /* * Initialise the HRT. */ diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index e79d7e10a3..dc28f446b4 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,6 +720,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { + hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -839,7 +840,12 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { - call->deadline = deadline + call->period; + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } hrt_call_enter(call); } } @@ -906,5 +912,16 @@ hrt_latency_update(void) latency_counters[index]++; } +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} #endif /* HRT_TIMER */ From 70e56a3d54ec517d97b6f080badfab6c66c06f77 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:37 +1100 Subject: [PATCH 074/233] px4fmu2: enable SPI sensor DRDY pins --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 2527e4c143..c66c490a7f 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -83,6 +83,11 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + stm32_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI2 From bc8cfc8d9d29650cc2ec7627cf5cedae0b5ed47d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Dec 2013 11:21:27 +0100 Subject: [PATCH 075/233] Fix indendation in airspeed driver (no functional change) --- src/drivers/airspeed/airspeed.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 62c0d1f17c..c341aa2c6d 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - float _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; From 0349937a828392d50e736d32f4eec6242d65c9aa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 10:19:35 +1100 Subject: [PATCH 076/233] lsm303d: added detailed logging of accels on extremes this will log accel values and registers to /fs/microsd/lsm303d.log if any extreme values are seen --- src/drivers/lsm303d/lsm303d.cpp | 165 +++++++++++++++++++++++++++++++- 1 file changed, 163 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 356569f998..6da684c104 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,7 @@ #include #include #include +#include #include #include @@ -231,6 +233,16 @@ public: */ void print_registers(); + /** + * toggle logging + */ + void toggle_logging(); + + /** + * check for extreme accel values + */ + void check_extremes(const accel_report *arb); + protected: virtual int probe(); @@ -273,6 +285,7 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _extreme_values; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -283,6 +296,15 @@ private: uint8_t _reg7_expected; uint8_t _reg1_expected; + // accel logging + int _accel_log_fd; + bool _accel_logging_enabled; + uint64_t _last_extreme_us; + uint64_t _last_log_us; + uint64_t _last_log_sync_us; + uint64_t _last_log_reg_us; + uint64_t _last_log_alarm_us; + /** * Start automatic measurement. */ @@ -478,11 +500,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), + _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _reg1_expected(0), - _reg7_expected(0) + _reg7_expected(0), + _accel_log_fd(-1), + _accel_logging_enabled(false), + _last_log_us(0), + _last_log_sync_us(0), + _last_log_reg_us(0), + _last_log_alarm_us(0) { // enable debug() calls _debug_enabled = true; @@ -519,6 +548,9 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); + perf_free(_reg1_resets); + perf_free(_reg7_resets); + perf_free(_extreme_values); } int @@ -628,6 +660,101 @@ LSM303D::probe() return -EIO; } +#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" + +/** + check for extreme accelerometer values and log to a file on the SD card + */ +void +LSM303D::check_extremes(const accel_report *arb) +{ + const float extreme_threshold = 30; + bool is_extreme = (fabsf(arb->x) > extreme_threshold && + fabsf(arb->y) > extreme_threshold && + fabsf(arb->z) > extreme_threshold); + if (is_extreme) { + perf_count(_extreme_values); + // force accel logging on if we see extreme values + _accel_logging_enabled = true; + } + + if (! _accel_logging_enabled) { + // logging has been disabled by user, close + if (_accel_log_fd != -1) { + ::close(_accel_log_fd); + _accel_log_fd = -1; + } + return; + } + if (_accel_log_fd == -1) { + // keep last 10 logs + ::unlink(ACCEL_LOGFILE ".9"); + for (uint8_t i=8; i>0; i--) { + uint8_t len = strlen(ACCEL_LOGFILE)+3; + char log1[len], log2[len]; + snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); + snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); + ::rename(log1, log2); + } + ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); + + // open the new logfile + _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); + if (_accel_log_fd == -1) { + return; + } + } + + uint64_t now = hrt_absolute_time(); + // log accels at 1Hz + if (now - _last_log_us > 1000*1000) { + _last_log_us = now; + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + (unsigned long long)arb->timestamp, + arb->x, arb->y, arb->z, + (int)arb->x_raw, + (int)arb->y_raw, + (int)arb->z_raw); + } + + // log registers at 10Hz when we have extreme values, or 0.5 Hz without + if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + (now - _last_log_reg_us > 10*1000*1000)) { + _last_log_reg_us = now; + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR }; + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + for (uint8_t i=0; i 10*1000*1000) { + _last_log_sync_us = now; + ::fsync(_accel_log_fd); + } + + // play alarm every 10s if we have had an extreme value + if (perf_event_count(_extreme_values) != 0 && + (now - _last_log_alarm_us > 10*1000*1000)) { + _last_log_alarm_us = now; + int tfd = ::open(TONEALARM_DEVICE_PATH, 0); + if (tfd != -1) { + ::ioctl(tfd, TONE_SET_ALARM, 4); + ::close(tfd); + } + } +} + ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { @@ -646,6 +773,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { + check_extremes(arb); ret += sizeof(*arb); arb++; } @@ -1495,6 +1623,18 @@ LSM303D::print_registers() printf("_reg7_expected=0x%02x\n", _reg7_expected); } +void +LSM303D::toggle_logging() +{ + if (! _accel_logging_enabled) { + _accel_logging_enabled = true; + printf("Started logging to %s\n", ACCEL_LOGFILE); + } else { + _accel_logging_enabled = false; + printf("Stopped logging\n"); + } +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", MAG_DEVICE_PATH), _parent(parent) @@ -1548,6 +1688,7 @@ void test(); void reset(); void info(); void regdump(); +void logging(); /** * Start the driver. @@ -1734,6 +1875,20 @@ regdump() exit(0); } +/** + * toggle logging + */ +void +logging() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->toggle_logging(); + + exit(0); +} + } // namespace @@ -1771,5 +1926,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "logging")) + lsm303d::logging(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); } From 895dc3a2bbf172c7a2095d3a815cdd13d1388816 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:39:07 +1100 Subject: [PATCH 077/233] lsm303d: dump I2C control registers in regdump --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6da684c104..fc81c72a55 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1574,6 +1574,8 @@ LSM303D::print_registers() const char *name; } regmap[] = { { ADDR_WHO_AM_I, "WHO_AM_I" }, + { 0x02, "I2C_CONTROL1" }, + { 0x15, "I2C_CONTROL2" }, { ADDR_STATUS_A, "STATUS_A" }, { ADDR_STATUS_M, "STATUS_M" }, { ADDR_CTRL_REG0, "CTRL_REG0" }, From b2b9665e4460ce25718509b7abb75c4b702c4c5f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:43:54 +1100 Subject: [PATCH 078/233] device: added register_class_devname() API this allows drivers to register generic device names for a device class, with automatic class instance handling --- src/drivers/device/cdev.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/drivers/device/device.h | 22 ++++++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 47ebcd40a7..7954ce5ab5 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -108,6 +108,42 @@ CDev::~CDev() unregister_driver(_devname); } +int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + int class_instance = 0; + int ret = -ENOSPC; + while (class_instance < 4) { + if (class_instance == 0) { + ret = register_driver(class_devname, &fops, 0666, (void *)this); + if (ret == OK) break; + } else { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + ret = register_driver(name, &fops, 0666, (void *)this); + if (ret == OK) break; + } + class_instance++; + } + if (class_instance == 4) + return ret; + return class_instance; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + if (class_instance > 0) { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + return unregister_driver(name); + } + return unregister_driver(class_devname); +} + int CDev::init() { diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index a9ed5d77c2..0235f62844 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -396,6 +396,25 @@ protected: */ virtual int close_last(struct file *filp); + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + private: static const unsigned _max_pollwaiters = 8; @@ -488,4 +507,7 @@ private: } // namespace device +// class instance for primary driver of each class +#define CLASS_DEVICE_PRIMARY 0 + #endif /* _DEVICE_DEVICE_H */ From 5ee41bc0835df045593cba50d90ef004cfec3167 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:44:58 +1100 Subject: [PATCH 079/233] hmc5883: use register_class_devname() --- src/drivers/hmc5883/hmc5883.cpp | 35 ++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22ad301ff3..d3b99ae66e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -155,6 +155,7 @@ private: float _range_scale; float _range_ga; bool _collect_phase; + int _class_instance; orb_advert_t _mag_topic; @@ -322,6 +323,7 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _mag_topic(-1), + _class_instance(-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")), @@ -352,6 +354,9 @@ HMC5883::~HMC5883() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -375,13 +380,17 @@ HMC5883::init() /* reset the device configuration */ reset(); - /* get a publish handle on the mag topic */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + _class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the mag topic if we are + * the primary mag */ + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } ret = OK; /* sensor is ok, but not calibrated */ @@ -876,8 +885,10 @@ HMC5883::collect() } #endif - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } /* post a report to the ring */ if (_reports->force(&new_report)) { @@ -1292,7 +1303,7 @@ test() int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1389,10 +1400,10 @@ int calibrate() { int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1414,7 +1425,7 @@ int calibrate() void reset() { - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); From 5a88dc02a7a7e6386f3987794afd8e3881abe0b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:10 +1100 Subject: [PATCH 080/233] l3gd20: use register_class_devname() --- src/drivers/l3gd20/l3gd20.cpp | 37 +++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a27bc52803..78a086b110 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,8 @@ #include #include +#define L3GD20_DEVICE_PATH "/dev/l3gd20" + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -199,6 +201,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + int _class_instance; unsigned _current_rate; unsigned _orientation; @@ -317,6 +320,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), @@ -346,6 +350,9 @@ L3GD20::~L3GD20() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _class_instance); + /* delete the perf counter */ perf_free(_sample_perf); } @@ -365,17 +372,13 @@ L3GD20::init() if (_reports == nullptr) goto out; - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + _class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic */ + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + } reset(); @@ -743,7 +746,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -922,7 +925,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; @@ -931,7 +934,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(GYRO_DEVICE_PATH, O_RDONLY); + fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -963,10 +966,10 @@ test() ssize_t sz; /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", L3GD20_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -999,7 +1002,7 @@ test() void reset() { - int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); From e334377e6c74f01a2bd75e435f911bb2dc29f401 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:28 +1100 Subject: [PATCH 081/233] lsm303d: use register_class_devname() --- src/drivers/lsm303d/lsm303d.cpp | 121 ++++++++++++++++---------------- 1 file changed, 62 insertions(+), 59 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 13ef682b4d..cb4b0b5d10 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,8 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -277,7 +277,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_advert_t _mag_topic; + int _class_instance; unsigned _accel_read; unsigned _mag_read; @@ -466,6 +466,8 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class LSM303D; @@ -473,6 +475,9 @@ protected: private: LSM303D *_parent; + orb_advert_t _mag_topic; + int _mag_class_instance; + void measure(); void measure_trampoline(void *arg); @@ -494,7 +499,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _mag_topic(-1), + _class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -544,6 +549,9 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; + if (_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + delete _mag; /* delete the perf counter */ @@ -573,10 +581,6 @@ LSM303D::init() goto out; /* advertise accel topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -584,53 +588,22 @@ LSM303D::init() reset(); - /* register the first instance as plain name, the 2nd as two and counting */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); + /* do CDev init for the mag device node */ + ret = _mag->init(); + if (ret != OK) { + warnx("MAG init failed"); + goto out; } - /* try to claim the generic accel node as well - it's OK if we fail at this */ - mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); - - if (mag_ret == OK) { - log("default mag device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary accel device, so advertise to + // the ORB + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); } - /* advertise mag topic */ - struct mag_report zero_mag_report; - memset(&zero_mag_report, 0, sizeof(zero_mag_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); - - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); - - if (mag_ret != OK) { - _mag_topic = -1; - } - - ret = OK; out: return ret; } @@ -1510,8 +1483,10 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (_accel_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } _accel_read++; @@ -1582,8 +1557,10 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + if (_mag->_mag_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } _mag_read++; @@ -1673,13 +1650,39 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", "/dev/lsm303d_mag"), - _parent(parent) + CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(-1), + _mag_class_instance(-1) { } LSM303D_mag::~LSM303D_mag() { + if (_mag_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance); +} + +int +LSM303D_mag::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) + goto out; + + _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary mag device, so advertise to + // the ORB + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + } + +out: + return ret; } void @@ -1739,7 +1742,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1858,7 +1861,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1869,7 +1872,7 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); From b55403c551070d681b9b438a0b52deb5832adc8f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:39 +1100 Subject: [PATCH 082/233] mpu6000: use register_class_devname() --- src/drivers/mpu6000/mpu6000.cpp | 114 ++++++++++++++++++++------------ 1 file changed, 71 insertions(+), 43 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ce00107521..6145a51175 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -211,16 +211,17 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _accel_class_instance; RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - unsigned _reads; unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; perf_counter_t _sample_perf; math::LowPassFilter2p _accel_filter_x; @@ -349,12 +350,17 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class MPU6000; void parent_poll_notify(); + private: MPU6000 *_parent; + orb_advert_t _gyro_topic; + int _gyro_class_instance; }; @@ -370,12 +376,13 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _reads(0), _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -420,8 +427,13 @@ MPU6000::~MPU6000() if (_gyro_reports != nullptr) delete _gyro_reports; + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); + /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); } int @@ -466,38 +478,23 @@ MPU6000::init() _gyro_scale.z_scale = 1.0f; /* do CDev init for the gyro device node, keep it optional */ - gyro_ret = _gyro->init(); + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } /* fetch an initial set of measurements for advertisement */ measure(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - if (gyro_ret != OK) { - _gyro_topic = -1; - } else { - gyro_report gr; - _gyro_reports->get(&gr); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } - - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + } out: return ret; @@ -677,6 +674,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) if (_accel_reports->empty()) return -EAGAIN; + perf_count(_accel_reads); + /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; @@ -694,12 +693,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) int MPU6000::self_test() { - if (_reads == 0) { + if (perf_event_count(_sample_perf) == 0) { measure(); } /* return 0 on success, 1 else */ - return (_reads > 0) ? 0 : 1; + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; } int @@ -771,6 +770,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) if (_gyro_reports->empty()) return -EAGAIN; + perf_count(_gyro_reads); + /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; @@ -1180,9 +1181,6 @@ MPU6000::measure() if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; - /* count measurement */ - _reads++; - /* * Convert from big to little endian */ @@ -1287,10 +1285,11 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + if (_accel_topic != -1) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + if (_gyro->_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1301,19 +1300,48 @@ void MPU6000::print_info() { perf_print_counter(_sample_perf); - printf("reads: %u\n", _reads); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), - _parent(parent) + _parent(parent), + _gyro_class_instance(-1) { } MPU6000_gyro::~MPU6000_gyro() { + if (_gyro_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance); +} + +int +MPU6000_gyro::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + gyro_report gr; + memset(&gr, 0, sizeof(gr)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + +out: + return ret; } void From 1bac7e7f8b073a7e5cee12570e42694988df1abc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:31 +1100 Subject: [PATCH 083/233] l3gd20: close fds before exit --- src/drivers/l3gd20/l3gd20.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 78a086b110..910131c6a0 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -942,6 +942,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -990,6 +992,8 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + close(fd_gyro); + /* XXX add poll-rate tests here too */ reset(); @@ -1013,6 +1017,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + exit(0); } From 038ec194ae55c94d566a62f9e6e7dbcc0e0e7399 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:43 +1100 Subject: [PATCH 084/233] lsm303d: close fds before exit --- src/drivers/lsm303d/lsm303d.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index cb4b0b5d10..4d7c8f05ac 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1770,6 +1770,8 @@ start() } } + close(fd); + close(fd_mag); exit(0); fail: @@ -1851,6 +1853,9 @@ test() /* XXX add poll-rate tests here too */ + close(fd_accel); + close(fd_mag); + reset(); errx(0, "PASS"); } @@ -1872,6 +1877,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { @@ -1882,6 +1889,8 @@ reset() err(1, "mag pollrate reset failed"); } + close(fd); + exit(0); } From 39b40e41c2126cb2aeba586a57333ebb2aa3c2a6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:54 +1100 Subject: [PATCH 085/233] mpu6000: close fds before exit --- src/drivers/mpu6000/mpu6000.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6145a51175..ff921274b8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1405,6 +1405,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -1508,6 +1510,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); + close(fd); + exit(0); } From 8744aa7536d2c52417855bd28ef589b72a42e5de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Dec 2013 10:03:50 +1100 Subject: [PATCH 086/233] ms5611: check for all zero in the prom when SPI CLK fails we get all zero data --- src/drivers/ms5611/ms5611_spi.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e9dff5a8bc..bc4074c55e 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -236,9 +236,12 @@ MS5611_SPI::_read_prom() usleep(3000); /* read and convert PROM words */ + bool all_zero = true; for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + if (_prom.c[i] != 0) + all_zero = false; //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } @@ -247,6 +250,10 @@ MS5611_SPI::_read_prom() if (ret != OK) { debug("crc failed"); } + if (all_zero) { + debug("prom all zero"); + ret = -EIO; + } return ret; } From 2b491a7954b8bbcca044790f314c78e0c91019df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 09:13:38 +1100 Subject: [PATCH 087/233] mpu6000: treat all zero data from mpu6k as bad --- src/drivers/mpu6000/mpu6000.cpp | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ff921274b8..c95d11c83b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -223,6 +223,7 @@ private: perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -384,6 +385,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -434,6 +436,7 @@ MPU6000::~MPU6000() perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); + perf_free(_bad_transfers); } int @@ -1013,9 +1016,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) uint8_t MPU6000::read_reg(unsigned reg) { - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1028,9 +1029,7 @@ MPU6000::read_reg(unsigned reg) uint16_t MPU6000::read_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1195,6 +1194,20 @@ MPU6000::measure() report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + return; + } + + /* * Swap axes and negate y */ From 893d66d9613c699c9e891e23bc78a61fcc5ad7b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 14:03:16 +1100 Subject: [PATCH 088/233] l3gd20: added rescheduling and error checking --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 910131c6a0..1077eb43b1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -209,6 +209,8 @@ private: unsigned _read; perf_counter_t _sample_perf; + perf_counter_t _reschedules; + perf_counter_t _errors; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -325,6 +327,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), + _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) @@ -355,6 +359,8 @@ L3GD20::~L3GD20() /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_reschedules); + perf_free(_errors); } int @@ -746,7 +752,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -776,6 +782,17 @@ L3GD20::measure_trampoline(void *arg) void L3GD20::measure() { +#ifdef GPIO_EXTI_GYRO_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + return; + } +#endif + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -798,6 +815,16 @@ L3GD20::measure() raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +#ifdef GPIO_EXTI_GYRO_DRDY + if (raw_report.status & 0xF != 0xF) { + /* + we waited for DRDY, but did not see DRDY on all axes + when we captured. That means a transfer error of some sort + */ + perf_count(_errors); + return; + } +#endif /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) From b3f4b0a240bfb9d7dfaafd78d0a6bbca1c429f60 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 15:09:36 +1100 Subject: [PATCH 089/233] drv_hrt: added note on why an uninitialised hrt_call is safe --- src/drivers/stm32/drv_hrt.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index dc28f446b4..1bd251bc2a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,7 +720,6 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { - hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -734,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); From 1b1aa0edea9214d5e7bab2db634b4fdbf8ad3e95 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:52:31 +1100 Subject: [PATCH 090/233] lsm303d: use DRDY pins to automatically reschedule measurements this prevents double reads of sensor data, and missing samples from the accel --- src/drivers/lsm303d/lsm303d.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4d7c8f05ac..b13ca9f44c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -287,6 +287,7 @@ private: perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; perf_counter_t _extreme_values; + perf_counter_t _accel_reschedules; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -507,6 +508,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), + _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -635,6 +637,8 @@ LSM303D::reset() _reg7_expected = REG7_CONT_MODE_M; write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); @@ -1416,6 +1420,14 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + return; + } if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { perf_count(_reg1_resets); reset(); From 93f3398dfedd787419f54294adc6c92c30e282b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 10:06:43 +1100 Subject: [PATCH 091/233] lsm303d: added 'lsm303d regdump' command useful for diagnosing issues --- src/drivers/lsm303d/lsm303d.cpp | 53 ++++++++++++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 60601e22c8..c21a255223 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -201,6 +201,11 @@ public: */ void print_info(); + /** + * dump register values + */ + void print_registers(); + protected: virtual int probe(); @@ -1380,6 +1385,30 @@ LSM303D::print_info() _mag_reports->print_info("mag reports"); } +void +LSM303D::print_registers() +{ + const struct { + uint8_t reg; + const char *name; + } regmap[] = { + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + }; + for (uint8_t i=0; iprint_registers(); + + exit(0); +} + } // namespace @@ -1634,5 +1679,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "info")) lsm303d::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "regdump")) + lsm303d::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From af049f7cf8b261aaa85a659537d75c1476da40f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:17:53 +1100 Subject: [PATCH 092/233] lsm303d: define some more register addresses --- src/drivers/lsm303d/lsm303d.cpp | 47 +++++++++++++++++++++++++-------- 1 file changed, 36 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c21a255223..de227974ec 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -82,19 +82,24 @@ static const int ERROR = -1; /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 +#define WHO_I_AM 0x49 -#define ADDR_OUT_L_T 0x05 -#define ADDR_OUT_H_T 0x06 -#define ADDR_STATUS_M 0x07 -#define ADDR_OUT_X_L_M 0x08 -#define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x0A -#define ADDR_OUT_Y_H_M 0x0B -#define ADDR_OUT_Z_L_M 0x0C -#define ADDR_OUT_Z_H_M 0x0D +#define ADDR_OUT_TEMP_L 0x05 +#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_INT_CTRL_M 0x12 +#define ADDR_INT_SRC_M 0x13 +#define ADDR_REFERENCE_X 0x1c +#define ADDR_REFERENCE_Y 0x1d +#define ADDR_REFERENCE_Z 0x1e -#define ADDR_OUT_TEMP_A 0x26 #define ADDR_STATUS_A 0x27 #define ADDR_OUT_X_L_A 0x28 #define ADDR_OUT_X_H_A 0x29 @@ -112,6 +117,26 @@ static const int ERROR = -1; #define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 +#define ADDR_FIFO_CTRL 0x2e +#define ADDR_FIFO_SRC 0x2f + +#define ADDR_IG_CFG1 0x30 +#define ADDR_IG_SRC1 0x31 +#define ADDR_IG_THS1 0x32 +#define ADDR_IG_DUR1 0x33 +#define ADDR_IG_CFG2 0x34 +#define ADDR_IG_SRC2 0x35 +#define ADDR_IG_THS2 0x36 +#define ADDR_IG_DUR2 0x37 +#define ADDR_CLICK_CFG 0x38 +#define ADDR_CLICK_SRC 0x39 +#define ADDR_CLICK_THS 0x3a +#define ADDR_TIME_LIMIT 0x3b +#define ADDR_TIME_LATENCY 0x3c +#define ADDR_TIME_WINDOW 0x3d +#define ADDR_ACT_THS 0x3e +#define ADDR_ACT_DUR 0x3f + #define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) From 415417196bcafdafa6aafa8109487ed0ed18cab6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 18:12:50 +1100 Subject: [PATCH 093/233] lsm303d: print more registers in "lsm303d regdump" --- src/drivers/lsm303d/lsm303d.cpp | 55 ++++++++++++++++++++++++++------- 1 file changed, 44 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index de227974ec..91f1fe0059 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1417,21 +1417,54 @@ LSM303D::print_registers() uint8_t reg; const char *name; } regmap[] = { - { ADDR_WHO_AM_I, "WHO_AM_I" }, - { ADDR_STATUS_A, "STATUS_A" }, - { ADDR_STATUS_M, "STATUS_M" }, - { ADDR_CTRL_REG0, "CTRL_REG0" }, - { ADDR_CTRL_REG1, "CTRL_REG1" }, - { ADDR_CTRL_REG2, "CTRL_REG2" }, - { ADDR_CTRL_REG3, "CTRL_REG3" }, - { ADDR_CTRL_REG4, "CTRL_REG4" }, - { ADDR_CTRL_REG5, "CTRL_REG5" }, - { ADDR_CTRL_REG6, "CTRL_REG6" }, - { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_OUT_TEMP_L, "TEMP_L" }, + { ADDR_OUT_TEMP_H, "TEMP_H" }, + { ADDR_INT_CTRL_M, "INT_CTRL_M" }, + { ADDR_INT_SRC_M, "INT_SRC_M" }, + { ADDR_REFERENCE_X, "REFERENCE_X" }, + { ADDR_REFERENCE_Y, "REFERENCE_Y" }, + { ADDR_REFERENCE_Z, "REFERENCE_Z" }, + { ADDR_OUT_X_L_A, "ACCEL_XL" }, + { ADDR_OUT_X_H_A, "ACCEL_XH" }, + { ADDR_OUT_Y_L_A, "ACCEL_YL" }, + { ADDR_OUT_Y_H_A, "ACCEL_YH" }, + { ADDR_OUT_Z_L_A, "ACCEL_ZL" }, + { ADDR_OUT_Z_H_A, "ACCEL_ZH" }, + { ADDR_FIFO_CTRL, "FIFO_CTRL" }, + { ADDR_FIFO_SRC, "FIFO_SRC" }, + { ADDR_IG_CFG1, "IG_CFG1" }, + { ADDR_IG_SRC1, "IG_SRC1" }, + { ADDR_IG_THS1, "IG_THS1" }, + { ADDR_IG_DUR1, "IG_DUR1" }, + { ADDR_IG_CFG2, "IG_CFG2" }, + { ADDR_IG_SRC2, "IG_SRC2" }, + { ADDR_IG_THS2, "IG_THS2" }, + { ADDR_IG_DUR2, "IG_DUR2" }, + { ADDR_CLICK_CFG, "CLICK_CFG" }, + { ADDR_CLICK_SRC, "CLICK_SRC" }, + { ADDR_CLICK_THS, "CLICK_THS" }, + { ADDR_TIME_LIMIT, "TIME_LIMIT" }, + { ADDR_TIME_LATENCY,"TIME_LATENCY" }, + { ADDR_TIME_WINDOW, "TIME_WINDOW" }, + { ADDR_ACT_THS, "ACT_THS" }, + { ADDR_ACT_DUR, "ACT_DUR" } }; for (uint8_t i=0; i Date: Wed, 30 Oct 2013 09:45:58 +1100 Subject: [PATCH 094/233] SPI: added set_frequency() API this allows the bus speed to be changed on the fly by device drivers. This is needed for the MPU6000 --- src/drivers/device/spi.cpp | 6 ++++++ src/drivers/device/spi.h | 11 +++++++++++ 2 files changed, 17 insertions(+) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fa6b78d640..fed6c312c1 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) return OK; } +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9103dca2ea..299575dc5c 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Set the SPI bus frequency + * This is used to change frequency on the fly. Some sensors + * (such as the MPU6000) need a lower frequency for setup + * registers and can handle higher frequency for sensor + * value registers + * + * @param frequency Frequency to set (Hz) + */ + void set_frequency(uint32_t frequency); + /** * Locking modes supported by the driver. */ From 97af3d22040e67520a835102684a1b2a9575aaaa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:46:52 +1100 Subject: [PATCH 095/233] mpu6000: change bus speed based on registers being accessed this ensures we follow the datasheet requirement of 1MHz for general registers and up to 20MHz for sensor and int status registers --- src/drivers/mpu6000/mpu6000.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 70359110ea..6bfa583fb0 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -161,6 +161,14 @@ #define MPU6000_ONE_G 9.80665f +/* + the MPU6000 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define MPU6000_LOW_BUS_SPEED 1000*1000 +#define MPU6000_HIGH_BUS_SPEED 10*1000*1000 + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -351,7 +359,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), + SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -991,6 +999,9 @@ MPU6000::read_reg(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; @@ -1003,6 +1014,9 @@ MPU6000::read_reg16(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; @@ -1016,6 +1030,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, nullptr, sizeof(cmd)); } @@ -1139,6 +1156,10 @@ MPU6000::measure() * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + // sensor transfer at high clock speed + set_frequency(MPU6000_HIGH_BUS_SPEED); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; From 50d5241985792d829833f287f2fc1936d8caef84 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 15:27:48 +1100 Subject: [PATCH 096/233] px4io: moved blue heartbeat LED to main loop this allows us to tell if the main loop is running by looking for a blinking blue LED --- src/modules/px4iofirmware/px4io.c | 13 +++++++++++++ src/modules/px4iofirmware/safety.c | 16 +--------------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index ff9eecd74f..cd9bd197ba 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -117,6 +117,13 @@ show_debug_messages(void) } } +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + int user_start(int argc, char *argv[]) { @@ -201,6 +208,7 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ @@ -216,6 +224,11 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); + if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 95335f038a..cdb54a80ad 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -77,7 +77,6 @@ static unsigned blink_counter = 0; static bool safety_button_pressed; static void safety_check_button(void *arg); -static void heartbeat_blink(void *arg); static void failsafe_blink(void *arg); void @@ -86,9 +85,6 @@ safety_init(void) /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); - /* arrange for the heartbeat handler to be called at 4Hz */ - hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); - /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -163,16 +159,6 @@ safety_check_button(void *arg) } } -static void -heartbeat_blink(void *arg) -{ - static bool heartbeat = false; - - /* XXX add flags here that need to be frobbed by various loops */ - - LED_BLUE(heartbeat = !heartbeat); -} - static void failsafe_blink(void *arg) { @@ -192,4 +178,4 @@ failsafe_blink(void *arg) } LED_AMBER(failsafe); -} \ No newline at end of file +} From cdaafff6e45b2d68d20248fd00ec157b47ff6f78 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 10:19:35 +1100 Subject: [PATCH 097/233] lsm303d: added detailed logging of accels on extremes this will log accel values and registers to /fs/microsd/lsm303d.log if any extreme values are seen --- src/drivers/lsm303d/lsm303d.cpp | 165 +++++++++++++++++++++++++++++++- 1 file changed, 163 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 91f1fe0059..e7869527cf 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,7 @@ #include #include #include +#include #include #include @@ -231,6 +233,16 @@ public: */ void print_registers(); + /** + * toggle logging + */ + void toggle_logging(); + + /** + * check for extreme accel values + */ + void check_extremes(const accel_report *arb); + protected: virtual int probe(); @@ -273,6 +285,7 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _extreme_values; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -283,6 +296,15 @@ private: uint8_t _reg7_expected; uint8_t _reg1_expected; + // accel logging + int _accel_log_fd; + bool _accel_logging_enabled; + uint64_t _last_extreme_us; + uint64_t _last_log_us; + uint64_t _last_log_sync_us; + uint64_t _last_log_reg_us; + uint64_t _last_log_alarm_us; + /** * Start automatic measurement. */ @@ -473,11 +495,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), + _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _reg1_expected(0), - _reg7_expected(0) + _reg7_expected(0), + _accel_log_fd(-1), + _accel_logging_enabled(false), + _last_log_us(0), + _last_log_sync_us(0), + _last_log_reg_us(0), + _last_log_alarm_us(0) { // enable debug() calls _debug_enabled = true; @@ -514,6 +543,9 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); + perf_free(_reg1_resets); + perf_free(_reg7_resets); + perf_free(_extreme_values); } int @@ -602,6 +634,101 @@ LSM303D::probe() return -EIO; } +#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" + +/** + check for extreme accelerometer values and log to a file on the SD card + */ +void +LSM303D::check_extremes(const accel_report *arb) +{ + const float extreme_threshold = 30; + bool is_extreme = (fabsf(arb->x) > extreme_threshold && + fabsf(arb->y) > extreme_threshold && + fabsf(arb->z) > extreme_threshold); + if (is_extreme) { + perf_count(_extreme_values); + // force accel logging on if we see extreme values + _accel_logging_enabled = true; + } + + if (! _accel_logging_enabled) { + // logging has been disabled by user, close + if (_accel_log_fd != -1) { + ::close(_accel_log_fd); + _accel_log_fd = -1; + } + return; + } + if (_accel_log_fd == -1) { + // keep last 10 logs + ::unlink(ACCEL_LOGFILE ".9"); + for (uint8_t i=8; i>0; i--) { + uint8_t len = strlen(ACCEL_LOGFILE)+3; + char log1[len], log2[len]; + snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); + snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); + ::rename(log1, log2); + } + ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); + + // open the new logfile + _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); + if (_accel_log_fd == -1) { + return; + } + } + + uint64_t now = hrt_absolute_time(); + // log accels at 1Hz + if (now - _last_log_us > 1000*1000) { + _last_log_us = now; + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + (unsigned long long)arb->timestamp, + arb->x, arb->y, arb->z, + (int)arb->x_raw, + (int)arb->y_raw, + (int)arb->z_raw); + } + + // log registers at 10Hz when we have extreme values, or 0.5 Hz without + if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + (now - _last_log_reg_us > 10*1000*1000)) { + _last_log_reg_us = now; + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR }; + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + for (uint8_t i=0; i 10*1000*1000) { + _last_log_sync_us = now; + ::fsync(_accel_log_fd); + } + + // play alarm every 10s if we have had an extreme value + if (perf_event_count(_extreme_values) != 0 && + (now - _last_log_alarm_us > 10*1000*1000)) { + _last_log_alarm_us = now; + int tfd = ::open(TONEALARM_DEVICE_PATH, 0); + if (tfd != -1) { + ::ioctl(tfd, TONE_SET_ALARM, 4); + ::close(tfd); + } + } +} + ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { @@ -620,6 +747,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { + check_extremes(arb); ret += sizeof(*arb); arb++; } @@ -1467,6 +1595,18 @@ LSM303D::print_registers() printf("_reg7_expected=0x%02x\n", _reg7_expected); } +void +LSM303D::toggle_logging() +{ + if (! _accel_logging_enabled) { + _accel_logging_enabled = true; + printf("Started logging to %s\n", ACCEL_LOGFILE); + } else { + _accel_logging_enabled = false; + printf("Stopped logging\n"); + } +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", MAG_DEVICE_PATH), _parent(parent) @@ -1520,6 +1660,7 @@ void test(); void reset(); void info(); void regdump(); +void logging(); /** * Start the driver. @@ -1706,6 +1847,20 @@ regdump() exit(0); } +/** + * toggle logging + */ +void +logging() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->toggle_logging(); + + exit(0); +} + } // namespace @@ -1743,5 +1898,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "logging")) + lsm303d::logging(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); } From 671447ce2c94ad523ea9dc636c5066db98831d87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 11:12:08 +1100 Subject: [PATCH 098/233] lsm303d: fixed TEMP_H register define --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e7869527cf..bbf70c1541 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -87,7 +87,7 @@ static const int ERROR = -1; #define WHO_I_AM 0x49 #define ADDR_OUT_TEMP_L 0x05 -#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_OUT_TEMP_H 0x06 #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 From 5ef91d694b94c297bfecae297b2c933561e0ac16 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 13:39:59 +1100 Subject: [PATCH 099/233] lsm303d: log mag regs too --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index bbf70c1541..c20c820aa9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -703,7 +703,9 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR }; + ADDR_ACT_THS, ADDR_ACT_DUR, + ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Fri, 15 Nov 2013 14:12:24 +1100 Subject: [PATCH 100/233] lsm303d: always log first ARB and REG values --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c20c820aa9..6d7b3df500 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -681,7 +681,8 @@ LSM303D::check_extremes(const accel_report *arb) uint64_t now = hrt_absolute_time(); // log accels at 1Hz - if (now - _last_log_us > 1000*1000) { + if (_last_log_us == 0 || + now - _last_log_us > 1000*1000) { _last_log_us = now; ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", (unsigned long long)arb->timestamp, @@ -692,7 +693,8 @@ LSM303D::check_extremes(const accel_report *arb) } // log registers at 10Hz when we have extreme values, or 0.5 Hz without - if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + if (_last_log_reg_us == 0 || + (is_extreme && (now - _last_log_reg_us > 500*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, From ea33a19c8f5b109fd9ba35603b0af56dddef3708 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Nov 2013 18:36:48 +1100 Subject: [PATCH 101/233] lsm303d: show regs at both high and low bus speed on error --- src/drivers/lsm303d/lsm303d.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6d7b3df500..2d150cdba5 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -708,11 +708,20 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_ACT_THS, ADDR_ACT_DUR, ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; - ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "FREG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Sat, 16 Nov 2013 18:37:26 +1100 Subject: [PATCH 102/233] lsm303d: zero-fill register reads --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2d150cdba5..bfa1293641 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1123,6 +1123,7 @@ LSM303D::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); From b927974a976a86271826a04f0020be01007368ad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Nov 2013 20:56:15 +1100 Subject: [PATCH 103/233] FMUv2: added support for MPU6000 on v2.4 board --- src/drivers/boards/px4fmu-v2/board_config.h | 2 ++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ec8dde4993..534394274c 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -85,11 +85,13 @@ __BEGIN_DECLS #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 09838d02fe..2527e4c143 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, * required for some peripheral @@ -81,6 +82,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); #endif #ifdef CONFIG_STM32_SPI2 @@ -99,6 +101,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_ACCEL_MAG: @@ -106,6 +109,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_BARO: @@ -113,6 +117,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: From 3ce14497a118fc6a10c475704dfa5dbdd9669476 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:28:42 +1100 Subject: [PATCH 104/233] FMUv2: added define for MPU DRDY pin --- src/drivers/boards/px4fmu-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 534394274c..9f66d195d7 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -79,6 +79,7 @@ __BEGIN_DECLS #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) #define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) #define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) From 44b2543d2d968a61b2b7cbef6d4409dcdf5c9174 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:02 +1100 Subject: [PATCH 105/233] FMUv2: set MPU6000 CS as initially de-selected --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index ae2a645f70..31ad60bd34 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\n"); From 9a169d8ef453aea1729b76940b10733591c4c6d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:33 +1100 Subject: [PATCH 106/233] l3gd20: added I2C disable based on method from ST engineering support --- src/drivers/l3gd20/l3gd20.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 8f56748234..31e38fbd9a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -218,6 +218,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -574,6 +579,7 @@ L3GD20::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); @@ -699,9 +705,19 @@ L3GD20::stop() hrt_cancel(&_call); } +void +L3GD20::disable_i2c(void) +{ + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); +} + void L3GD20::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ @@ -753,6 +769,7 @@ L3GD20::measure() perf_begin(_sample_perf); /* fetch data from the sensor */ + memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); From 7c9d92a5d64ee4b2282139a49e189df434ec00f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:30:04 +1100 Subject: [PATCH 107/233] lsm303d: added I2C disable based on method from ST engineering support --- src/drivers/lsm303d/lsm303d.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index bfa1293641..da981989c4 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -322,6 +322,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -595,9 +600,25 @@ out: return ret; } +void +LSM303D::disable_i2c(void) +{ + uint8_t a = read_reg(0x02); + write_reg(0x02, (0x10 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xF7 & a)); + a = read_reg(0x15); + write_reg(0x15, (0x80 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xE7 & a)); +} + void LSM303D::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* enable accel*/ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; write_reg(ADDR_CTRL_REG1, _reg1_expected); From a2b31118cb4dc01174c4c3436c29d5850d116441 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:30:48 +1100 Subject: [PATCH 108/233] lsm303d: get cleaner logic traces by gathering all regs more regularly --- src/drivers/lsm303d/lsm303d.cpp | 42 +++++++++++++++------------------ 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index da981989c4..0c0aa0ff52 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -713,36 +713,32 @@ LSM303D::check_extremes(const accel_report *arb) (int)arb->z_raw); } + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR, + ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; + uint8_t regval[sizeof(reglist)]; + for (uint8_t i=0; i 500*1000)) || + (is_extreme && (now - _last_log_reg_us > 250*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; - const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, - ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, - ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, - ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, - ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, - ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, - ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR, - ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; - ::dprintf(_accel_log_fd, "FREG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Thu, 28 Nov 2013 20:31:28 +1100 Subject: [PATCH 109/233] lsm303d: cleanup logic traces by pre-zeroing all transfers --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 0c0aa0ff52..576bc184e1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1437,6 +1437,7 @@ LSM303D::measure() perf_begin(_accel_sample_perf); /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); @@ -1514,6 +1515,7 @@ LSM303D::mag_measure() perf_begin(_mag_sample_perf); /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); From aeba9e5c1e59b016b6ce707852490714db0544e2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:31:51 +1100 Subject: [PATCH 110/233] FMUv2: change CS pins to 2MHz this gives cleaner traces --- src/drivers/boards/px4fmu-v2/board_config.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 9f66d195d7..586661b58f 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -82,11 +82,11 @@ __BEGIN_DECLS #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) -#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 From 51a1ad48c5734f259bc8c90b2b5f5626b83cbdbb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:33:32 +1100 Subject: [PATCH 111/233] FMUv2: don't config ADC pins that are now used for MPU6k CS and other uses --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 31ad60bd34..269ec238eb 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ From c46ab017e12f5ccbe0f22ec77f90d8a8e28a8c63 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 12:06:05 +1100 Subject: [PATCH 112/233] lsm303d: make log distinctive with i2c disable included --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 576bc184e1..5c9a87eaa8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -734,7 +734,7 @@ LSM303D::check_extremes(const accel_report *arb) (is_extreme && (now - _last_log_reg_us > 250*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; - ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Wed, 4 Dec 2013 15:50:41 +1100 Subject: [PATCH 113/233] lsm303d: changed tones for accel fail to 3 tones distinct tones for init fail, post-boot fail and recovery --- src/drivers/lsm303d/lsm303d.cpp | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 5c9a87eaa8..44cb469128 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -664,6 +664,7 @@ void LSM303D::check_extremes(const accel_report *arb) { const float extreme_threshold = 30; + static bool boot_ok = false; bool is_extreme = (fabsf(arb->x) > extreme_threshold && fabsf(arb->y) > extreme_threshold && fabsf(arb->z) > extreme_threshold); @@ -671,7 +672,9 @@ LSM303D::check_extremes(const accel_report *arb) perf_count(_extreme_values); // force accel logging on if we see extreme values _accel_logging_enabled = true; - } + } else { + boot_ok = true; + } if (! _accel_logging_enabled) { // logging has been disabled by user, close @@ -705,15 +708,16 @@ LSM303D::check_extremes(const accel_report *arb) if (_last_log_us == 0 || now - _last_log_us > 1000*1000) { _last_log_us = now; - ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", (unsigned long long)arb->timestamp, arb->x, arb->y, arb->z, (int)arb->x_raw, (int)arb->y_raw, - (int)arb->z_raw); + (int)arb->z_raw, + (unsigned)boot_ok); } - const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, @@ -723,7 +727,7 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, ADDR_ACT_THS, ADDR_ACT_DUR, ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I}; uint8_t regval[sizeof(reglist)]; for (uint8_t i=0; i Date: Fri, 1 Nov 2013 08:11:58 +1100 Subject: [PATCH 114/233] lsm303d: init filter to 773 Hz --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 44cb469128..a38524c220 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -631,7 +631,7 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); From 0a83772c0d8b4b600245590d571f679a6894e75f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 16:12:25 +1100 Subject: [PATCH 115/233] l3gd20: use highest possible on-chip filter bandwidth this allows the software filter to do its job properly --- src/drivers/l3gd20/l3gd20.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 31e38fbd9a..103b26ac58 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -92,9 +92,12 @@ static const int ERROR = -1; #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -659,16 +662,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_25HZ; + bits |= RATE_190HZ_LP_70HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_20HZ; + bits |= RATE_380HZ_LP_100HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_30HZ; - + bits |= RATE_760HZ_LP_100HZ; } else { return -EINVAL; } @@ -732,7 +734,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(L3GD20_DEFAULT_RATE); + set_samplerate(0); // 760Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); From 24a243843ef771273c81536c0bd18c5d70c010f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 22:54:02 +1100 Subject: [PATCH 116/233] lsm303d/l3gd20: change filters to 50Hz analog on-chip filters after discussion with Leonard these analog on-chip filters should be at 50Hz --- src/drivers/l3gd20/l3gd20.cpp | 11 ++++++++--- src/drivers/lsm303d/lsm303d.cpp | 7 ++++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 103b26ac58..17b53bfa96 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -93,10 +93,15 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 @@ -662,15 +667,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_70HZ; + bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_100HZ; + bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_100HZ; + bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a38524c220..701947b981 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -631,7 +631,12 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz + + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); From 513d014f03831111a1b9f9af293e253f7d5218a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Dec 2013 09:06:53 +1100 Subject: [PATCH 117/233] l3gd20: added retries to disable_i2c() --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 17b53bfa96..176ef648bb 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -715,8 +715,16 @@ L3GD20::stop() void L3GD20::disable_i2c(void) { - uint8_t a = read_reg(0x05); - write_reg(0x05, (0x20 | a)); + uint8_t retries = 10; + while (retries--) { + // add retries + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); + if (read_reg(0x05) == (a | 0x20)) { + return; + } + } + debug("FAILED TO DISABLE I2C"); } void From 4956feffdfc8459c5f27e471b50978db29f23c07 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:00 +1100 Subject: [PATCH 118/233] drv_hrt: added hrt_call_init() and hrt_call_delay() APIs hrt_call_init() can be used to initialise (zero) a hrt_call structure to ensure safe usage. The hrt_call_every() interface calls this automatically. hrt_call_delay() can be used to delay a current callout by the given number of microseconds --- src/drivers/drv_hrt.h | 14 ++++++++++++++ src/drivers/stm32/drv_hrt.c | 19 ++++++++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8a99eeca79..d130d68b34 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry); */ __EXPORT extern void hrt_cancel(struct hrt_call *entry); +/* + * initialise a hrt_call structure + */ +__EXPORT extern void hrt_call_init(struct hrt_call *entry); + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); + /* * Initialise the HRT. */ diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index e79d7e10a3..dc28f446b4 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,6 +720,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { + hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -839,7 +840,12 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { - call->deadline = deadline + call->period; + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } hrt_call_enter(call); } } @@ -906,5 +912,16 @@ hrt_latency_update(void) latency_counters[index]++; } +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} #endif /* HRT_TIMER */ From 0e97c288bbe5acde6358e10237d1bfb5e522ee19 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:37 +1100 Subject: [PATCH 119/233] px4fmu2: enable SPI sensor DRDY pins --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 2527e4c143..c66c490a7f 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -83,6 +83,11 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + stm32_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI2 From 30ff61fa90179af00609738da6bd5365872d4f61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:52:31 +1100 Subject: [PATCH 120/233] lsm303d: use DRDY pins to automatically reschedule measurements this prevents double reads of sensor data, and missing samples from the accel --- src/drivers/lsm303d/lsm303d.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 701947b981..6010a2ad73 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -286,6 +286,7 @@ private: perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; perf_counter_t _extreme_values; + perf_counter_t _accel_reschedules; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -501,6 +502,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), + _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -627,6 +629,8 @@ LSM303D::reset() _reg7_expected = REG7_CONT_MODE_M; write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); @@ -1430,6 +1434,14 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + return; + } if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { perf_count(_reg1_resets); reset(); From 7e309414758d2c526da7ef3bcab7bf75779f6950 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:53:25 +1100 Subject: [PATCH 121/233] ms5611: change bus speed to 5MHz this gives 5MHz SPI bus speed (by asking for 6MHz due to timer granularity). Tests with a logic analyser show that the ms5611 is actually more reliable at 5MHz than lower speeds --- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e547c913ba..33e5be940d 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000), _prom(prom_buf) { } From bc6ddb971fa23b88679ea8201a8205be3c08e90c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:54:58 +1100 Subject: [PATCH 122/233] ms5611: removed unused variable --- src/drivers/ms5611/ms5611_spi.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 33e5be940d..8dd89120d2 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,7 +134,6 @@ int MS5611_SPI::init() { int ret; - irqstate_t flags; ret = SPI::init(); if (ret != OK) { From 0456ee2364600ba6b5a9f109c7464a71579e7d58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:55:10 +1100 Subject: [PATCH 123/233] ms5611: give cleaner SPI traces this makes logic traces cleaner by zeroing extra bytes written --- src/drivers/ms5611/ms5611_spi.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 8dd89120d2..e9dff5a8bc 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -166,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[4]; + uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 }; /* read the most recent measurement */ - buf[0] = 0 | DIR_WRITE; int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { @@ -240,18 +239,21 @@ MS5611_SPI::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + if (ret != OK) { + debug("crc failed"); + } + return ret; } uint16_t MS5611_SPI::_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; _transfer(cmd, cmd, sizeof(cmd)); From 3d27dd7246c660bb0a00caae3cd0b0e66bfa6467 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:34:25 +0100 Subject: [PATCH 124/233] Made all usual suspects default to their custom names and only register the default name if its not already taken by someone else --- src/drivers/hmc5883/hmc5883.cpp | 7 ++++--- src/drivers/l3gd20/l3gd20.cpp | 7 +++++++ src/drivers/lsm303d/lsm303d.cpp | 33 ++++++++++++++++++++++++--------- src/drivers/mpu6000/mpu6000.cpp | 33 +++++++++++++++++++++++++-------- 4 files changed, 60 insertions(+), 20 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5f0ce4ff86..22ad301ff3 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,6 +77,7 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 +#define HMC5883L_DEVICE_PATH "/dev/hmc5883" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -315,7 +316,7 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : - I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1256,7 +1257,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -1288,7 +1289,7 @@ test() ssize_t sz; int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 176ef648bb..a27bc52803 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -365,6 +365,13 @@ L3GD20::init() if (_reports == nullptr) goto out; + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + /* advertise sensor topic */ struct gyro_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6010a2ad73..a3409cbac1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,7 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) - +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" +#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -585,6 +586,20 @@ LSM303D::init() reset(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); + + if (mag_ret == OK) { + log("default mag device"); + } + /* advertise mag topic */ struct mag_report zero_mag_report; memset(&zero_mag_report, 0, sizeof(zero_mag_report)); @@ -1670,7 +1685,7 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), + CDev("LSM303D_mag", "/dev/lsm303d_mag"), _parent(parent) { } @@ -1736,7 +1751,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1747,7 +1762,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1755,7 +1770,7 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); /* don't fail if open cannot be opened */ if (0 <= fd_mag) { @@ -1790,10 +1805,10 @@ test() int ret; /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); @@ -1819,10 +1834,10 @@ test() struct mag_report m_report; /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6bfa583fb0..ce00107521 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -75,6 +75,9 @@ #define DIR_READ 0x80 #define DIR_WRITE 0x00 +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" + // MPU 6000 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 @@ -359,7 +362,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -468,6 +471,20 @@ MPU6000::init() /* fetch an initial set of measurements for advertisement */ measure(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + if (gyro_ret != OK) { _gyro_topic = -1; } else { @@ -1290,7 +1307,7 @@ MPU6000::print_info() } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", GYRO_DEVICE_PATH), + CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent) { } @@ -1352,7 +1369,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1384,17 +1401,17 @@ test() ssize_t sz; /* get the driver */ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + MPU_DEVICE_PATH_ACCEL); /* get the driver */ - int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1452,7 +1469,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); From 1fb406ba094a091c6976f752c3b6db71fc895605 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:44:29 +0100 Subject: [PATCH 125/233] Add also default descriptor for alternate sensors --- src/drivers/lsm303d/lsm303d.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a3409cbac1..94966a9637 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -586,11 +586,21 @@ LSM303D::init() reset(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ + /* register the first instance as plain name, the 2nd as two and counting */ ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default accel device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* try to claim the generic accel node as well - it's OK if we fail at this */ @@ -598,6 +608,16 @@ LSM303D::init() if (mag_ret == OK) { log("default mag device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* advertise mag topic */ From f24479c27a3627d315eee0f329da8aca8741eebe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:39:07 +1100 Subject: [PATCH 126/233] lsm303d: dump I2C control registers in regdump --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 94966a9637..c535c4e871 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1643,6 +1643,8 @@ LSM303D::print_registers() const char *name; } regmap[] = { { ADDR_WHO_AM_I, "WHO_AM_I" }, + { 0x02, "I2C_CONTROL1" }, + { 0x15, "I2C_CONTROL2" }, { ADDR_STATUS_A, "STATUS_A" }, { ADDR_STATUS_M, "STATUS_M" }, { ADDR_CTRL_REG0, "CTRL_REG0" }, From 6145e69fc62c7448baf0531d5c492e26b9225b01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:43:54 +1100 Subject: [PATCH 127/233] device: added register_class_devname() API this allows drivers to register generic device names for a device class, with automatic class instance handling --- src/drivers/device/cdev.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/drivers/device/device.h | 22 ++++++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 47ebcd40a7..7954ce5ab5 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -108,6 +108,42 @@ CDev::~CDev() unregister_driver(_devname); } +int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + int class_instance = 0; + int ret = -ENOSPC; + while (class_instance < 4) { + if (class_instance == 0) { + ret = register_driver(class_devname, &fops, 0666, (void *)this); + if (ret == OK) break; + } else { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + ret = register_driver(name, &fops, 0666, (void *)this); + if (ret == OK) break; + } + class_instance++; + } + if (class_instance == 4) + return ret; + return class_instance; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + if (class_instance > 0) { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + return unregister_driver(name); + } + return unregister_driver(class_devname); +} + int CDev::init() { diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index a9ed5d77c2..0235f62844 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -396,6 +396,25 @@ protected: */ virtual int close_last(struct file *filp); + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + private: static const unsigned _max_pollwaiters = 8; @@ -488,4 +507,7 @@ private: } // namespace device +// class instance for primary driver of each class +#define CLASS_DEVICE_PRIMARY 0 + #endif /* _DEVICE_DEVICE_H */ From c5097a6561928e9d5e88926e5be28df845256d91 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:44:58 +1100 Subject: [PATCH 128/233] hmc5883: use register_class_devname() --- src/drivers/hmc5883/hmc5883.cpp | 35 ++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22ad301ff3..d3b99ae66e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -155,6 +155,7 @@ private: float _range_scale; float _range_ga; bool _collect_phase; + int _class_instance; orb_advert_t _mag_topic; @@ -322,6 +323,7 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _mag_topic(-1), + _class_instance(-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")), @@ -352,6 +354,9 @@ HMC5883::~HMC5883() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -375,13 +380,17 @@ HMC5883::init() /* reset the device configuration */ reset(); - /* get a publish handle on the mag topic */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + _class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the mag topic if we are + * the primary mag */ + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } ret = OK; /* sensor is ok, but not calibrated */ @@ -876,8 +885,10 @@ HMC5883::collect() } #endif - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } /* post a report to the ring */ if (_reports->force(&new_report)) { @@ -1292,7 +1303,7 @@ test() int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1389,10 +1400,10 @@ int calibrate() { int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1414,7 +1425,7 @@ int calibrate() void reset() { - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); From 1d5f0a1433f838fc553461c808129a89d917058a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:10 +1100 Subject: [PATCH 129/233] l3gd20: use register_class_devname() --- src/drivers/l3gd20/l3gd20.cpp | 37 +++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a27bc52803..78a086b110 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,8 @@ #include #include +#define L3GD20_DEVICE_PATH "/dev/l3gd20" + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -199,6 +201,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + int _class_instance; unsigned _current_rate; unsigned _orientation; @@ -317,6 +320,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), @@ -346,6 +350,9 @@ L3GD20::~L3GD20() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _class_instance); + /* delete the perf counter */ perf_free(_sample_perf); } @@ -365,17 +372,13 @@ L3GD20::init() if (_reports == nullptr) goto out; - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + _class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic */ + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + } reset(); @@ -743,7 +746,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -922,7 +925,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; @@ -931,7 +934,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(GYRO_DEVICE_PATH, O_RDONLY); + fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -963,10 +966,10 @@ test() ssize_t sz; /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", L3GD20_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -999,7 +1002,7 @@ test() void reset() { - int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); From 02e7f7fa85ec4d443e32cd320d80a4152712a22c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:28 +1100 Subject: [PATCH 130/233] lsm303d: use register_class_devname() --- src/drivers/lsm303d/lsm303d.cpp | 121 ++++++++++++++++---------------- 1 file changed, 62 insertions(+), 59 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c535c4e871..beac3c7510 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,8 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -277,7 +277,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_advert_t _mag_topic; + int _class_instance; unsigned _accel_read; unsigned _mag_read; @@ -467,6 +467,8 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class LSM303D; @@ -474,6 +476,9 @@ protected: private: LSM303D *_parent; + orb_advert_t _mag_topic; + int _mag_class_instance; + void measure(); void measure_trampoline(void *arg); @@ -495,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _mag_topic(-1), + _class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -546,6 +551,9 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; + if (_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + delete _mag; /* delete the perf counter */ @@ -575,10 +583,6 @@ LSM303D::init() goto out; /* advertise accel topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -586,53 +590,22 @@ LSM303D::init() reset(); - /* register the first instance as plain name, the 2nd as two and counting */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); + /* do CDev init for the mag device node */ + ret = _mag->init(); + if (ret != OK) { + warnx("MAG init failed"); + goto out; } - /* try to claim the generic accel node as well - it's OK if we fail at this */ - mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); - - if (mag_ret == OK) { - log("default mag device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary accel device, so advertise to + // the ORB + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); } - /* advertise mag topic */ - struct mag_report zero_mag_report; - memset(&zero_mag_report, 0, sizeof(zero_mag_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); - - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); - - if (mag_ret != OK) { - _mag_topic = -1; - } - - ret = OK; out: return ret; } @@ -1544,8 +1517,10 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (_accel_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } _accel_read++; @@ -1616,8 +1591,10 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + if (_mag->_mag_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } _mag_read++; @@ -1707,13 +1684,39 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", "/dev/lsm303d_mag"), - _parent(parent) + CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(-1), + _mag_class_instance(-1) { } LSM303D_mag::~LSM303D_mag() { + if (_mag_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance); +} + +int +LSM303D_mag::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) + goto out; + + _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary mag device, so advertise to + // the ORB + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + } + +out: + return ret; } void @@ -1773,7 +1776,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1892,7 +1895,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1903,7 +1906,7 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); From 1fc122562c16dfd419e832b95292678a7db8ec22 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:39 +1100 Subject: [PATCH 131/233] mpu6000: use register_class_devname() --- src/drivers/mpu6000/mpu6000.cpp | 114 ++++++++++++++++++++------------ 1 file changed, 71 insertions(+), 43 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ce00107521..6145a51175 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -211,16 +211,17 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _accel_class_instance; RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - unsigned _reads; unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; perf_counter_t _sample_perf; math::LowPassFilter2p _accel_filter_x; @@ -349,12 +350,17 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class MPU6000; void parent_poll_notify(); + private: MPU6000 *_parent; + orb_advert_t _gyro_topic; + int _gyro_class_instance; }; @@ -370,12 +376,13 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _reads(0), _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -420,8 +427,13 @@ MPU6000::~MPU6000() if (_gyro_reports != nullptr) delete _gyro_reports; + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); + /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); } int @@ -466,38 +478,23 @@ MPU6000::init() _gyro_scale.z_scale = 1.0f; /* do CDev init for the gyro device node, keep it optional */ - gyro_ret = _gyro->init(); + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } /* fetch an initial set of measurements for advertisement */ measure(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - if (gyro_ret != OK) { - _gyro_topic = -1; - } else { - gyro_report gr; - _gyro_reports->get(&gr); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } - - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + } out: return ret; @@ -677,6 +674,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) if (_accel_reports->empty()) return -EAGAIN; + perf_count(_accel_reads); + /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; @@ -694,12 +693,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) int MPU6000::self_test() { - if (_reads == 0) { + if (perf_event_count(_sample_perf) == 0) { measure(); } /* return 0 on success, 1 else */ - return (_reads > 0) ? 0 : 1; + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; } int @@ -771,6 +770,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) if (_gyro_reports->empty()) return -EAGAIN; + perf_count(_gyro_reads); + /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; @@ -1180,9 +1181,6 @@ MPU6000::measure() if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; - /* count measurement */ - _reads++; - /* * Convert from big to little endian */ @@ -1287,10 +1285,11 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + if (_accel_topic != -1) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + if (_gyro->_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1301,19 +1300,48 @@ void MPU6000::print_info() { perf_print_counter(_sample_perf); - printf("reads: %u\n", _reads); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), - _parent(parent) + _parent(parent), + _gyro_class_instance(-1) { } MPU6000_gyro::~MPU6000_gyro() { + if (_gyro_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance); +} + +int +MPU6000_gyro::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + gyro_report gr; + memset(&gr, 0, sizeof(gr)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + +out: + return ret; } void From 09ece4306e929b9ffbd7d21a50c5d3d21265bd87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:31 +1100 Subject: [PATCH 132/233] l3gd20: close fds before exit --- src/drivers/l3gd20/l3gd20.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 78a086b110..910131c6a0 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -942,6 +942,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -990,6 +992,8 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + close(fd_gyro); + /* XXX add poll-rate tests here too */ reset(); @@ -1013,6 +1017,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + exit(0); } From acd0a70dca1066e09fc98ed6576682b6de330e7c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:43 +1100 Subject: [PATCH 133/233] lsm303d: close fds before exit --- src/drivers/lsm303d/lsm303d.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index beac3c7510..11e5b95a7e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1804,6 +1804,8 @@ start() } } + close(fd); + close(fd_mag); exit(0); fail: @@ -1885,6 +1887,9 @@ test() /* XXX add poll-rate tests here too */ + close(fd_accel); + close(fd_mag); + reset(); errx(0, "PASS"); } @@ -1906,6 +1911,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { @@ -1916,6 +1923,8 @@ reset() err(1, "mag pollrate reset failed"); } + close(fd); + exit(0); } From f0d84d4826a6563693ae0abd6ac1f72a3eafdc68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:54 +1100 Subject: [PATCH 134/233] mpu6000: close fds before exit --- src/drivers/mpu6000/mpu6000.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6145a51175..ff921274b8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1405,6 +1405,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -1508,6 +1510,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); + close(fd); + exit(0); } From 96881d88106c421a4ea44118b754f90d47b94e4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Dec 2013 10:03:50 +1100 Subject: [PATCH 135/233] ms5611: check for all zero in the prom when SPI CLK fails we get all zero data --- src/drivers/ms5611/ms5611_spi.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e9dff5a8bc..bc4074c55e 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -236,9 +236,12 @@ MS5611_SPI::_read_prom() usleep(3000); /* read and convert PROM words */ + bool all_zero = true; for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + if (_prom.c[i] != 0) + all_zero = false; //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } @@ -247,6 +250,10 @@ MS5611_SPI::_read_prom() if (ret != OK) { debug("crc failed"); } + if (all_zero) { + debug("prom all zero"); + ret = -EIO; + } return ret; } From 91870953d951bc0e15640363bcc1d826cb5ed1b1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 09:13:38 +1100 Subject: [PATCH 136/233] mpu6000: treat all zero data from mpu6k as bad --- src/drivers/mpu6000/mpu6000.cpp | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ff921274b8..c95d11c83b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -223,6 +223,7 @@ private: perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -384,6 +385,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -434,6 +436,7 @@ MPU6000::~MPU6000() perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); + perf_free(_bad_transfers); } int @@ -1013,9 +1016,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) uint8_t MPU6000::read_reg(unsigned reg) { - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1028,9 +1029,7 @@ MPU6000::read_reg(unsigned reg) uint16_t MPU6000::read_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1195,6 +1194,20 @@ MPU6000::measure() report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + return; + } + + /* * Swap axes and negate y */ From d43e3394b07b5999dff5d04a3dd77f96d76c1134 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 14:03:16 +1100 Subject: [PATCH 137/233] l3gd20: added rescheduling and error checking --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 910131c6a0..1077eb43b1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -209,6 +209,8 @@ private: unsigned _read; perf_counter_t _sample_perf; + perf_counter_t _reschedules; + perf_counter_t _errors; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -325,6 +327,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), + _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) @@ -355,6 +359,8 @@ L3GD20::~L3GD20() /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_reschedules); + perf_free(_errors); } int @@ -746,7 +752,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -776,6 +782,17 @@ L3GD20::measure_trampoline(void *arg) void L3GD20::measure() { +#ifdef GPIO_EXTI_GYRO_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + return; + } +#endif + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -798,6 +815,16 @@ L3GD20::measure() raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +#ifdef GPIO_EXTI_GYRO_DRDY + if (raw_report.status & 0xF != 0xF) { + /* + we waited for DRDY, but did not see DRDY on all axes + when we captured. That means a transfer error of some sort + */ + perf_count(_errors); + return; + } +#endif /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) From cf78440ee6cb8c1469f53ca096d7388524facd59 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 15:09:36 +1100 Subject: [PATCH 138/233] drv_hrt: added note on why an uninitialised hrt_call is safe --- src/drivers/stm32/drv_hrt.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index dc28f446b4..1bd251bc2a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,7 +720,6 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { - hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -734,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); From b69097df387ed6cea65d9884f6b3bc2ecb3ce3d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Dec 2013 16:38:11 +0100 Subject: [PATCH 139/233] px4io frimware: improve handling of manual mode when fmu is still healthy, use data from fmu for channels which are not controlled by rc --- src/modules/px4iofirmware/mixer.cpp | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 87844ca70a..fdf01e09cb 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -77,7 +77,8 @@ enum mixer_source { MIX_NONE, MIX_FMU, MIX_OVERRIDE, - MIX_FAILSAFE + MIX_FAILSAFE, + MIX_OVERRIDE_FMU_OK }; static mixer_source source; @@ -135,10 +136,19 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; + } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; } } @@ -241,7 +251,7 @@ mixer_callback(uintptr_t handle, switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS) { + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } @@ -254,6 +264,17 @@ mixer_callback(uintptr_t handle, } return -1; + case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ + if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + break; + } + return -1; + case MIX_FAILSAFE: case MIX_NONE: control = 0.0f; From c033443208666d6972d99fe5a7b8e0c3fa5050b5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Dec 2013 16:57:19 +0100 Subject: [PATCH 140/233] px4iofirmware: improve check for rc controlled channels in manual mode --- src/modules/px4iofirmware/mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index fdf01e09cb..9fc86db5e7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -258,7 +258,7 @@ mixer_callback(uintptr_t handle, return -1; case MIX_OVERRIDE: - if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } @@ -266,7 +266,7 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE_FMU_OK: /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ - if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { From 17502cbde43cf96c55af1811723ce84ca2a1fc27 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Dec 2013 09:02:31 +1100 Subject: [PATCH 141/233] l3gd20: fixed a warning --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1077eb43b1..1d437df2ba 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -816,7 +816,7 @@ L3GD20::measure() transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); #ifdef GPIO_EXTI_GYRO_DRDY - if (raw_report.status & 0xF != 0xF) { + if ((raw_report.status & 0xF) != 0xF) { /* we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort From e808e015dd84c234f9689daf90aedf0162d7d2f2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Dec 2013 14:14:03 +1100 Subject: [PATCH 142/233] LowPassFilter: allow for filtering to be disabled using bandwidth of 0 gives no filtering --- src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index efb17225d2..3699d9bce3 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -46,6 +46,10 @@ namespace math void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) { _cutoff_freq = cutoff_freq; + if (_cutoff_freq <= 0.0f) { + // no filtering + return; + } float fr = sample_freq/_cutoff_freq; float ohm = tanf(M_PI_F/fr); float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; @@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) float LowPassFilter2p::apply(float sample) { + if (_cutoff_freq <= 0.0f) { + // no filtering + return sample; + } // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { From 8f90efa312b4bccbacb9e9173e2cba7d7b4bc193 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Dec 2013 14:14:33 +1100 Subject: [PATCH 143/233] l3gd20: print more perf counters and make DRDY usage clearer --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1d437df2ba..d639acba15 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -779,10 +779,16 @@ L3GD20::measure_trampoline(void *arg) dev->measure(); } +#ifdef GPIO_EXTI_GYRO_DRDY +# define L3GD20_USE_DRDY 1 +#else +# define L3GD20_USE_DRDY 0 +#endif + void L3GD20::measure() { -#ifdef GPIO_EXTI_GYRO_DRDY +#if L3GD20_USE_DRDY // if the gyro doesn't have any data ready then re-schedule // for 100 microseconds later. This ensures we don't double // read a value and then miss the next value @@ -815,7 +821,7 @@ L3GD20::measure() raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); -#ifdef GPIO_EXTI_GYRO_DRDY +#if L3GD20_USE_DRDY if ((raw_report.status & 0xF) != 0xF) { /* we waited for DRDY, but did not see DRDY on all axes @@ -902,6 +908,8 @@ L3GD20::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); + perf_print_counter(_reschedules); + perf_print_counter(_errors); _reports->print_info("report queue"); } From 6016fbe55d3fd402012168b5a704a550bcfc4d28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Dec 2013 11:30:07 +1100 Subject: [PATCH 144/233] Merged PX4IO crc checks and force update --- src/drivers/px4io/px4io.cpp | 46 ++++++++++++++++++++++++++- src/drivers/px4io/px4io_uploader.cpp | 5 ++- src/modules/px4iofirmware/protocol.h | 3 ++ src/modules/px4iofirmware/registers.c | 26 +++++++++++++++ 4 files changed, 78 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ef6ca04e97..f5fb618c35 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -95,6 +95,7 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) #define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz @@ -2146,6 +2147,16 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; + case PX4IO_REBOOT_BOOTLOADER: + if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + return -EINVAL; + + /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); + // we don't expect a reply from this operation + ret = OK; + break; + case PX4IO_INAIR_RESTART_ENABLE: /* set/clear the 'in-air restart' bit */ @@ -2673,6 +2684,39 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "forceupdate")) { + /* + force update of the IO firmware without requiring + the user to hold the safety switch down + */ + if (argc <= 3) { + printf("usage: px4io forceupdate MAGIC filename\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + + // upload the specified firmware + const char *fn[2]; + fn[0] = argv[3]; + fn[1] = nullptr; + PX4IO_Uploader *up = new PX4IO_Uploader; + up->upload(&fn[0]); + delete up; + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || @@ -2690,5 +2734,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d01dedb0dd..41f93a8ee1 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -274,7 +274,10 @@ PX4IO_Uploader::drain() int ret; do { - ret = recv(c, 1000); + // the small recv timeout here is to allow for fast + // drain when rebooting the io board for a forced + // update of the fw without using the safety switch + ret = recv(c, 40); #ifdef UDEBUG if (ret == OK) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 5e5396782b..2856bdea4e 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -186,6 +186,9 @@ enum { /* DSM bind states */ /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 86a40bc228..6785e53661 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -45,6 +45,8 @@ #include #include +#include +#include #include "px4io.h" #include "protocol.h" @@ -154,6 +156,7 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_VBATT_SCALE] = 10000, #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, + [PX4IO_P_SETUP_REBOOT_BL] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) @@ -501,6 +504,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_REBOOT_BL: + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // don't allow reboot while armed + break; + } + + // check the magic value + if (value != PX4IO_REBOOT_BL_MAGIC) + break; + + // note that we don't set BL_WAIT_MAGIC in + // BKP_DR1 as that is not necessary given the + // timing of the forceupdate command. The + // bootloader on px4io waits for enough time + // anyway, and this method works with older + // bootloader versions (tested with both + // revision 3 and revision 4). + + up_systemreset(); + break; + case PX4IO_P_SETUP_DSM: dsm_bind(value & 0x0f, (value >> 4) & 7); break; From 5b7d1af5d83554d3119e3e0669d6429fc1aef262 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Dec 2013 12:44:11 +1100 Subject: [PATCH 145/233] Merged crccheck command --- src/drivers/px4io/px4io.cpp | 64 ++++++++++++++++++++++++++- src/modules/px4iofirmware/protocol.h | 2 + src/modules/px4iofirmware/px4io.c | 20 +++++++++ src/modules/px4iofirmware/registers.c | 1 + 4 files changed, 85 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f5fb618c35..cc3815f3ea 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -96,6 +97,7 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) #define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) +#define PX4IO_CHECK_CRC _IOC(0xff00, 3) #define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz @@ -1660,11 +1662,13 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u hardware %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), @@ -2157,6 +2161,19 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = OK; break; + case PX4IO_CHECK_CRC: { + /* check IO firmware CRC against passed value */ + uint32_t io_crc = 0; + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); + if (ret != OK) + return ret; + if (io_crc != arg) { + debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + return -EINVAL; + } + break; + } + case PX4IO_INAIR_RESTART_ENABLE: /* set/clear the 'in-air restart' bit */ @@ -2717,6 +2734,49 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "checkcrc")) { + /* + check IO CRC against CRC of a file + */ + if (argc <= 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + int fd = open(argv[2], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[2], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 2856bdea4e..cffabbb45d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -189,6 +189,8 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd9bd197ba..745bd5705f 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -124,6 +125,22 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } + +static void +calculate_fw_crc(void) +{ +#define APP_SIZE_MAX 0xf000 +#define APP_LOAD_ADDRESS 0x08001000 + // compute CRC of the current firmware + uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); + } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; + r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; +} + int user_start(int argc, char *argv[]) { @@ -136,6 +153,9 @@ user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); + /* calculate our fw CRC so FMU can decide if we need to update */ + calculate_fw_crc(); + /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6785e53661..1a8519aec9 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -157,6 +157,7 @@ volatile uint16_t r_page_setup[] = #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, + [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) From c46bd8b0413fcaea5b19777bf074f0d65417a47c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Dec 2013 20:49:05 +1100 Subject: [PATCH 146/233] Enabled DMA on both telemetry ports and GPS --- nuttx-configs/px4fmu-v2/nsh/defconfig | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 372453fb6d..db7a9c5c4e 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -299,9 +299,10 @@ CONFIG_STM32_USART=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set -# CONFIG_USART3_RXDMA is not set +CONFIG_USART3_RXDMA=y # CONFIG_UART4_RS485 is not set -# CONFIG_UART4_RXDMA is not set +CONFIG_UART4_RXDMA=y +# CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set # CONFIG_USART6_RXDMA is not set # CONFIG_UART7_RS485 is not set From 3a40ea8338206b247aadb40e3709a22e0699135b Mon Sep 17 00:00:00 2001 From: Holger Steinhaus L Date: Mon, 11 Nov 2013 12:14:03 +0100 Subject: [PATCH 147/233] more precise range conversion for SBus input signals --- src/modules/px4iofirmware/sbus.c | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index c523df6caf..8034e7077c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -55,6 +55,24 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 +/* + Measured values with Futaba FX-30/R6108SB: + -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) +*/ + +/* define range mapping here, -+100% -> 1000..2000 */ +#define SBUS_RANGE_MIN 350.0f +#define SBUS_RANGE_MAX 1700.0f + +#define SBUS_TARGET_MIN 1000.0f +#define SBUS_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) +#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) + static int sbus_fd = -1; static hrt_abstime last_rx_time; @@ -234,8 +252,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint } } - /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - values[channel] = (value / 2) + 998; + + /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ From a673fa3926d18163841f817c08bf089a0a8224f7 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus L Date: Sun, 17 Nov 2013 12:56:26 +0100 Subject: [PATCH 148/233] Non-destructive handling of failsafe signals, distinction between frame loss and signal loss. This kind of handling might be moved upstream into the application, once alarms are propagated by the ORB system. This change is compatible with RX failsafe settings, but does not rely on it (uses SBus flags instead). --- src/modules/px4iofirmware/sbus.c | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 8034e7077c..68af85dc97 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -54,6 +54,9 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 +#define SBUS_FLAGS_BYTE 23 +#define SBUS_FAILSAFE_BIT 3 +#define SBUS_FRAMELOST_BIT 2 /* Measured values with Futaba FX-30/R6108SB: @@ -220,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint return false; } - /* if the failsafe or connection lost bit is set, we consider the frame invalid */ - if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ - - /* actively announce signal loss */ - *values = 0; - return false; - } - /* we have received something we think is a frame */ last_frame_time = frame_time; @@ -262,13 +256,28 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint chancount = 18; /* channel 17 (index 16) */ - values[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - values[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ *num_values = chancount; + /* decode and handle failsafe and frame-lost flags */ + if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ + /* emulate throttle failsafe for maximum compatibility and do not destroy any channel data */ + values[2] = 900; + } + else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, + * e.g. by prematurely issueing return-to-launch!!! */ + + // values[2] = 888; // marker for debug purposes + } + return true; } From 9883a346a022131b20a292d96a87f20fb7921b1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Dec 2013 18:01:55 +0100 Subject: [PATCH 149/233] First stab at implementing better RSSI based connection status estimation, still needs some work and testing --- src/drivers/drv_rc_input.h | 3 +++ src/modules/px4iofirmware/controls.c | 21 +++++++++++++++++---- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/sbus.c | 17 ++++++++++------- 6 files changed, 33 insertions(+), 13 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 78ffad9d7b..86e5a149a0 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -89,6 +89,9 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */ + int32_t rssi; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5c621cfb2e..194d8aab98 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -94,6 +94,9 @@ controls_tick() { * other. Don't do that. */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */ + uint16_t rssi = 0; + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); @@ -104,14 +107,15 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; else r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + + rssi = 1000; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - r_raw_rc_count = 8; } perf_end(c_gather_sbus); @@ -122,10 +126,19 @@ controls_tick() { */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); - if (ppm_updated) + if (ppm_updated) { + + /* XXX sample RSSI properly here */ + rssi = 1000; + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + } perf_end(c_gather_ppm); + /* limit number of channels to allowable data size */ + if (r_raw_rc_count > PX4IO_INPUT_CHANNELS) + r_raw_rc_count = PX4IO_INPUT_CHANNELS; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -221,7 +234,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0) { + if (assigned_channels == 0 || rssi == 0) { rc_input_lost = true; } else { /* set RC OK flag */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index cffabbb45d..11e3803976 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -124,6 +124,7 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ +#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4fea0288c4..b9c9e02329 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -209,7 +209,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1a8519aec9..3f9e111baa 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -114,7 +114,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon }; /** diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 68af85dc97..2153fadc82 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, max_channels); + return sbus_decode(now, values, num_values, rssi, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -266,8 +266,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ - /* emulate throttle failsafe for maximum compatibility and do not destroy any channel data */ - values[2] = 900; + /* report that we failed to read anything valid off the receiver */ + *rssi = 0; + return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented @@ -276,8 +277,10 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - // values[2] = 888; // marker for debug purposes + *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) } + *rssi = 1000; + return true; } From 00dc339d2ece42161e36af7cc52c042c4fcec697 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 14:52:57 +0100 Subject: [PATCH 150/233] Improved S.Bus scaling based on scope measurements --- src/modules/px4iofirmware/sbus.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 2153fadc82..65e14a18f6 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,8 +66,8 @@ */ /* define range mapping here, -+100% -> 1000..2000 */ -#define SBUS_RANGE_MIN 350.0f -#define SBUS_RANGE_MAX 1700.0f +#define SBUS_RANGE_MIN 215.0f +#define SBUS_RANGE_MAX 1790.0f #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f From 10b2dc67e4529759b9dcbef6a18360eba610b5a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 14:54:02 +0100 Subject: [PATCH 151/233] Allow forceupdate in all conditions --- src/drivers/px4io/px4io.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5d35ecb8d8..685a9746d0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2653,22 +2653,22 @@ px4io_main(int argc, char *argv[]) the user to hold the safety switch down */ if (argc <= 3) { - printf("usage: px4io forceupdate MAGIC filename\n"); + warnx("usage: px4io forceupdate MAGIC filename"); exit(1); } if (g_dev == nullptr) { - printf("px4io is not started\n"); - exit(1); - } - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); - } + warnx("px4io is not started, still attempting upgrade"); + } else { + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } - // tear down the px4io instance - delete g_dev; + // tear down the px4io instance + delete g_dev; + } // upload the specified firmware const char *fn[2]; From ea10d55d71b25b4440c6b4dc139dda519a0d2797 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 15:08:56 +0100 Subject: [PATCH 152/233] Auto-update of IO firmware, shorter preflight check alarm --- ROMFS/px4fmu_common/init.d/rcS | 53 ++++++++++++++----- src/drivers/px4io/px4io.cpp | 2 +- .../preflight_check/preflight_check.c | 2 +- 3 files changed, 41 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f1df990487..f551c1aa8c 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,6 +8,8 @@ # set MODE autostart +set logfile /fs/microsd/bootlog.txt + # # Try to mount the microSD card. # @@ -147,26 +149,49 @@ then nshterm /dev/ttyACM0 & fi - # # Upgrade PX4IO firmware # - if px4io detect - then - echo "PX4IO running, not upgrading" - else - echo "Attempting to upgrade PX4IO" - if px4io update - then - if [ -d /fs/microsd ] - then - echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log - fi - # Allow IO to safely kick back to app + if [ -f /etc/extras/px4io-v2_default.bin ] + then + set io_file /etc/extras/px4io-v2_default.bin + else + set io_file /etc/extras/px4io-v1_default.bin + fi + + if px4io start + then + echo "PX4IO OK" + echo "PX4IO OK" >> $logfile + fi + + if px4io checkcrc $io_file + then + echo "PX4IO CRC OK" + echo "PX4IO CRC OK" >> $logfile + else + echo "PX4IO CRC failure" + echo "PX4IO CRC failure" >> $logfile + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file + then usleep 200000 + if px4io start + then + echo "PX4IO restart OK" + echo "PX4IO restart OK" >> $logfile + tone_alarm MSPAA + else + echo "PX4IO restart failed" + echo "PX4IO restart failed" >> $logfile + tone_alarm MNGGG + sh /etc/init.d/rc.error + fi else - echo "No PX4IO to upgrade here" + echo "PX4IO update failed" + echo "PX4IO update failed" >> $logfile + tone_alarm MNGGG fi fi diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 685a9746d0..569f1e4137 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2229,7 +2229,7 @@ void start(int argc, char *argv[]) { if (g_dev != nullptr) - errx(1, "already loaded"); + errx(0, "already loaded"); /* allocate the interface */ device::Device *interface = get_interface(); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 07581459b4..982b03782f 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -200,7 +200,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 50; i++) + for (int i = 0; i < 14; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); From d6a6d59d2de7270fd978fcb6029edc3f315899a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 15:09:20 +0100 Subject: [PATCH 153/233] Further improved S.Bus scaling --- src/modules/px4iofirmware/sbus.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 65e14a18f6..388502b403 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,8 +66,8 @@ */ /* define range mapping here, -+100% -> 1000..2000 */ -#define SBUS_RANGE_MIN 215.0f -#define SBUS_RANGE_MAX 1790.0f +#define SBUS_RANGE_MIN 200.0f +#define SBUS_RANGE_MAX 1800.0f #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f From f4ac204f4650dedcde52aa9ec001fafb5e7c5284 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Dec 2013 18:32:46 +0100 Subject: [PATCH 154/233] Cranking up bus speeds for all sensors to achievable 10.4 MHz, will cut the bus lock time to half --- src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/mpu6000/mpu6000.cpp | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index d639acba15..670e51b979 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -316,7 +316,7 @@ private: }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : - SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call_interval(0), _reports(nullptr), _gyro_range_scale(0.0f), diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 11e5b95a7e..969b5e25f9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -486,7 +486,7 @@ private: LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _call_accel_interval(0), _call_mag_interval(0), diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c95d11c83b..be4e422b04 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -170,7 +170,7 @@ SPI speed */ #define MPU6000_LOW_BUS_SPEED 1000*1000 -#define MPU6000_HIGH_BUS_SPEED 10*1000*1000 +#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ class MPU6000_gyro; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index bc4074c55e..26216e840f 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { } From b63d4809dedb965d2342ff5803e6604b11ff18c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Dec 2013 19:35:23 +0100 Subject: [PATCH 155/233] Enabled MPU6K and updated startup script to start all sensors --- ROMFS/px4fmu_common/init.d/rc.sensors | 16 +++++++++++----- makefiles/config_px4fmu-v2_default.mk | 1 + 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f17b650bc2..7f73acf859 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -19,12 +19,18 @@ fi if mpu6000 start then echo "using MPU6000" - set BOARD fmuv1 -else - echo "using L3GD20 and LSM303D" - l3gd20 start - lsm303d start +fi + +if l3gd20 start +then + echo "using L3GD20(H)" +fi + +if lsm303d start +then set BOARD fmuv2 +else + set BOARD fmuv1 fi # Start airspeed sensors diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 015a7387a6..68673c0552 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -21,6 +21,7 @@ MODULES += drivers/px4fmu MODULES += drivers/px4io MODULES += drivers/boards/px4fmu-v2 MODULES += drivers/rgbled +MODULES += drivers/mpu6000 MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 From da539ec83f30609700970ad7f06ad635e01ee658 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Dec 2013 09:01:56 +0100 Subject: [PATCH 156/233] Added LOG_ MAVLink messages --- .../v1.0/ardupilotmega/ardupilotmega.h | 23 +- .../mavlink_msg_rally_fetch_point.h | 221 +++ .../ardupilotmega/mavlink_msg_rally_point.h | 375 +++++ .../mavlink/v1.0/ardupilotmega/testsuite.h | 344 ++-- .../mavlink/v1.0/ardupilotmega/version.h | 2 +- .../include/mavlink/v1.0/autoquad/autoquad.h | 123 -- .../include/mavlink/v1.0/autoquad/mavlink.h | 27 - .../autoquad/mavlink_msg_aq_telemetry_f.h | 617 -------- .../include/mavlink/v1.0/autoquad/testsuite.h | 118 -- .../include/mavlink/v1.0/autoquad/version.h | 12 - mavlink/include/mavlink/v1.0/common/common.h | 46 +- .../v1.0/common/mavlink_msg_battery_status.h | 154 +- .../v1.0/common/mavlink_msg_gps_raw_int.h | 10 +- .../mavlink/v1.0/common/mavlink_msg_hil_gps.h | 10 +- .../v1.0/common/mavlink_msg_log_data.h | 237 +++ .../v1.0/common/mavlink_msg_log_entry.h | 265 ++++ .../v1.0/common/mavlink_msg_log_erase.h | 199 +++ .../common/mavlink_msg_log_request_data.h | 265 ++++ .../v1.0/common/mavlink_msg_log_request_end.h | 199 +++ .../common/mavlink_msg_log_request_list.h | 243 +++ .../v1.0/common/mavlink_msg_mission_item.h | 50 +- .../v1.0/common/mavlink_msg_scaled_imu2.h | 375 +++++ .../v1.0/common/mavlink_msg_sys_status.h | 30 +- .../include/mavlink/v1.0/common/testsuite.h | 1380 +++++++++++------ mavlink/include/mavlink/v1.0/common/version.h | 2 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 6 +- .../mavlink/v1.0/matrixpilot/testsuite.h | 282 ++-- .../mavlink/v1.0/matrixpilot/version.h | 2 +- .../include/mavlink/v1.0/pixhawk/pixhawk.h | 6 +- .../include/mavlink/v1.0/pixhawk/testsuite.h | 228 +-- .../include/mavlink/v1.0/pixhawk/version.h | 2 +- 31 files changed, 3952 insertions(+), 1901 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/autoquad.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/version.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 3aad8e6785..4dc9aed260 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, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 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, 8, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 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, 8, 48, 19, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 222, 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, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 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, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 222, 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, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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" @@ -79,6 +79,7 @@ enum MAV_CMD MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -100,7 +101,8 @@ enum FENCE_ACTION FENCE_ACTION_NONE=0, /* Disable fenced mode | */ FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ - FENCE_ACTION_ENUM_END=3, /* | */ + FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ + FENCE_ACTION_ENUM_END=4, /* | */ }; #endif @@ -144,6 +146,17 @@ enum LIMIT_MODULE }; #endif +/** @brief Flags in RALLY_POINT message */ +#ifndef HAVE_ENUM_RALLY_FLAGS +#define HAVE_ENUM_RALLY_FLAGS +enum RALLY_FLAGS +{ + FAVORABLE_WIND=1, /* Flag set when requiring favorable winds for landing. | */ + LAND_IMMEDIATELY=2, /* Flag set when plane is to immediately descend to break altitude and land without GCS intervention. Flag not set when plane is to loiter at Rally point until commanded to land. | */ + RALLY_FLAGS_ENUM_END=3, /* | */ +}; +#endif + #include "../common/common.h" // MAVLINK VERSION @@ -182,6 +195,8 @@ enum LIMIT_MODULE #include "./mavlink_msg_data96.h" #include "./mavlink_msg_rangefinder.h" #include "./mavlink_msg_airspeed_autocal.h" +#include "./mavlink_msg_rally_point.h" +#include "./mavlink_msg_rally_fetch_point.h" #ifdef __cplusplus } diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h new file mode 100644 index 0000000000..f057e3c335 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h @@ -0,0 +1,221 @@ +// MESSAGE RALLY_FETCH_POINT PACKING + +#define MAVLINK_MSG_ID_RALLY_FETCH_POINT 176 + +typedef struct __mavlink_rally_fetch_point_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t idx; ///< point index (first point is 0) +} mavlink_rally_fetch_point_t; + +#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN 3 +#define MAVLINK_MSG_ID_176_LEN 3 + +#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC 234 +#define MAVLINK_MSG_ID_176_CRC 234 + + + +#define MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT { \ + "RALLY_FETCH_POINT", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rally_fetch_point_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rally_fetch_point_t, target_component) }, \ + { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_rally_fetch_point_t, idx) }, \ + } \ +} + + +/** + * @brief Pack a rally_fetch_point 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 target_component Component ID + * @param idx point index (first point is 0) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#else + mavlink_rally_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +} + +/** + * @brief Pack a rally_fetch_point 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#else + mavlink_rally_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +} + +/** + * @brief Encode a rally_fetch_point struct + * + * @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 rally_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) +{ + return mavlink_msg_rally_fetch_point_pack(system_id, component_id, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); +} + +/** + * @brief Encode a rally_fetch_point struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param rally_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) +{ + return mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, chan, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); +} + +/** + * @brief Send a rally_fetch_point message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#else + mavlink_rally_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#endif +} + +#endif + +// MESSAGE RALLY_FETCH_POINT UNPACKING + + +/** + * @brief Get field target_system from rally_fetch_point message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rally_fetch_point_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from rally_fetch_point message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rally_fetch_point_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field idx from rally_fetch_point message + * + * @return point index (first point is 0) + */ +static inline uint8_t mavlink_msg_rally_fetch_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a rally_fetch_point message into a struct + * + * @param msg The message to decode + * @param rally_fetch_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_rally_fetch_point_decode(const mavlink_message_t* msg, mavlink_rally_fetch_point_t* rally_fetch_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + rally_fetch_point->target_system = mavlink_msg_rally_fetch_point_get_target_system(msg); + rally_fetch_point->target_component = mavlink_msg_rally_fetch_point_get_target_component(msg); + rally_fetch_point->idx = mavlink_msg_rally_fetch_point_get_idx(msg); +#else + memcpy(rally_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h new file mode 100644 index 0000000000..2c21db4c0e --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h @@ -0,0 +1,375 @@ +// MESSAGE RALLY_POINT PACKING + +#define MAVLINK_MSG_ID_RALLY_POINT 175 + +typedef struct __mavlink_rally_point_t +{ + int32_t lat; ///< Latitude of point in degrees * 1E7 + int32_t lng; ///< Longitude of point in degrees * 1E7 + int16_t alt; ///< Transit / loiter altitude in meters relative to home + int16_t break_alt; ///< Break altitude in meters relative to home + uint16_t land_dir; ///< Heading to aim for when landing. In centi-degrees. + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t idx; ///< point index (first point is 0) + uint8_t count; ///< total number of points (for sanity checking) + uint8_t flags; ///< See RALLY_FLAGS enum for definition of the bitmask. +} mavlink_rally_point_t; + +#define MAVLINK_MSG_ID_RALLY_POINT_LEN 19 +#define MAVLINK_MSG_ID_175_LEN 19 + +#define MAVLINK_MSG_ID_RALLY_POINT_CRC 138 +#define MAVLINK_MSG_ID_175_CRC 138 + + + +#define MAVLINK_MESSAGE_INFO_RALLY_POINT { \ + "RALLY_POINT", \ + 10, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_rally_point_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_rally_point_t, lng) }, \ + { "alt", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rally_point_t, alt) }, \ + { "break_alt", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rally_point_t, break_alt) }, \ + { "land_dir", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rally_point_t, land_dir) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_rally_point_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_rally_point_t, target_component) }, \ + { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rally_point_t, idx) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rally_point_t, count) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_rally_point_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a rally_point 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 target_component Component ID + * @param idx point index (first point is 0) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point in degrees * 1E7 + * @param lng Longitude of point in degrees * 1E7 + * @param alt Transit / loiter altitude in meters relative to home + * @param break_alt Break altitude in meters relative to home + * @param land_dir Heading to aim for when landing. In centi-degrees. + * @param flags See RALLY_FLAGS enum for definition of the bitmask. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#else + mavlink_rally_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.break_alt = break_alt; + packet.land_dir = land_dir; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +} + +/** + * @brief Pack a rally_point 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point in degrees * 1E7 + * @param lng Longitude of point in degrees * 1E7 + * @param alt Transit / loiter altitude in meters relative to home + * @param break_alt Break altitude in meters relative to home + * @param land_dir Heading to aim for when landing. In centi-degrees. + * @param flags See RALLY_FLAGS enum for definition of the bitmask. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,int32_t lat,int32_t lng,int16_t alt,int16_t break_alt,uint16_t land_dir,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#else + mavlink_rally_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.break_alt = break_alt; + packet.land_dir = land_dir; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +} + +/** + * @brief Encode a rally_point struct + * + * @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 rally_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) +{ + return mavlink_msg_rally_point_pack(system_id, component_id, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); +} + +/** + * @brief Encode a rally_point struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param rally_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) +{ + return mavlink_msg_rally_point_pack_chan(system_id, component_id, chan, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); +} + +/** + * @brief Send a rally_point message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point in degrees * 1E7 + * @param lng Longitude of point in degrees * 1E7 + * @param alt Transit / loiter altitude in meters relative to home + * @param break_alt Break altitude in meters relative to home + * @param land_dir Heading to aim for when landing. In centi-degrees. + * @param flags See RALLY_FLAGS enum for definition of the bitmask. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#else + mavlink_rally_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.break_alt = break_alt; + packet.land_dir = land_dir; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#endif +} + +#endif + +// MESSAGE RALLY_POINT UNPACKING + + +/** + * @brief Get field target_system from rally_point message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rally_point_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field target_component from rally_point message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rally_point_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 15); +} + +/** + * @brief Get field idx from rally_point message + * + * @return point index (first point is 0) + */ +static inline uint8_t mavlink_msg_rally_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field count from rally_point message + * + * @return total number of points (for sanity checking) + */ +static inline uint8_t mavlink_msg_rally_point_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + +/** + * @brief Get field lat from rally_point message + * + * @return Latitude of point in degrees * 1E7 + */ +static inline int32_t mavlink_msg_rally_point_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lng from rally_point message + * + * @return Longitude of point in degrees * 1E7 + */ +static inline int32_t mavlink_msg_rally_point_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field alt from rally_point message + * + * @return Transit / loiter altitude in meters relative to home + */ +static inline int16_t mavlink_msg_rally_point_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field break_alt from rally_point message + * + * @return Break altitude in meters relative to home + */ +static inline int16_t mavlink_msg_rally_point_get_break_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field land_dir from rally_point message + * + * @return Heading to aim for when landing. In centi-degrees. + */ +static inline uint16_t mavlink_msg_rally_point_get_land_dir(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field flags from rally_point message + * + * @return See RALLY_FLAGS enum for definition of the bitmask. + */ +static inline uint8_t mavlink_msg_rally_point_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Decode a rally_point message into a struct + * + * @param msg The message to decode + * @param rally_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_rally_point_decode(const mavlink_message_t* msg, mavlink_rally_point_t* rally_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + rally_point->lat = mavlink_msg_rally_point_get_lat(msg); + rally_point->lng = mavlink_msg_rally_point_get_lng(msg); + rally_point->alt = mavlink_msg_rally_point_get_alt(msg); + rally_point->break_alt = mavlink_msg_rally_point_get_break_alt(msg); + rally_point->land_dir = mavlink_msg_rally_point_get_land_dir(msg); + rally_point->target_system = mavlink_msg_rally_point_get_target_system(msg); + rally_point->target_component = mavlink_msg_rally_point_get_target_component(msg); + rally_point->idx = mavlink_msg_rally_point_get_idx(msg); + rally_point->count = mavlink_msg_rally_point_get_count(msg); + rally_point->flags = mavlink_msg_rally_point_get_flags(msg); +#else + memcpy(rally_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index 2c9cfef3dc..eb3bc6a759 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -31,17 +31,17 @@ static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id, uint16_t i; mavlink_sensor_offsets_t packet_in = { 17.0, - 963497672, - 963497880, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 19107, - 19211, - 19315, + }963497672, + }963497880, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }19107, + }19211, + }19315, }; mavlink_sensor_offsets_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -96,10 +96,10 @@ static void mavlink_test_set_mag_offsets(uint8_t system_id, uint8_t component_id uint16_t i; mavlink_set_mag_offsets_t packet_in = { 17235, - 17339, - 17443, - 151, - 218, + }17339, + }17443, + }151, + }218, }; mavlink_set_mag_offsets_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -147,7 +147,7 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin uint16_t i; mavlink_meminfo_t packet_in = { 17235, - 17339, + }17339, }; mavlink_meminfo_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -192,11 +192,11 @@ static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink uint16_t i; mavlink_ap_adc_t packet_in = { 17235, - 17339, - 17443, - 17547, - 17651, - 17755, + }17339, + }17443, + }17547, + }17651, + }17755, }; mavlink_ap_adc_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -245,16 +245,16 @@ static void mavlink_test_digicam_configure(uint8_t system_id, uint8_t component_ uint16_t i; mavlink_digicam_configure_t packet_in = { 17.0, - 17443, - 151, - 218, - 29, - 96, - 163, - 230, - 41, - 108, - 175, + }17443, + }151, + }218, + }29, + }96, + }163, + }230, + }41, + }108, + }175, }; mavlink_digicam_configure_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -308,15 +308,15 @@ static void mavlink_test_digicam_control(uint8_t system_id, uint8_t component_id uint16_t i; mavlink_digicam_control_t packet_in = { 17.0, - 17, - 84, - 151, - 218, - 29, - 96, - 163, - 230, - 41, + }17, + }84, + }151, + }218, + }29, + }96, + }163, + }230, + }41, }; mavlink_digicam_control_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -369,11 +369,11 @@ static void mavlink_test_mount_configure(uint8_t system_id, uint8_t component_id uint16_t i; mavlink_mount_configure_t packet_in = { 5, - 72, - 139, - 206, - 17, - 84, + }72, + }139, + }206, + }17, + }84, }; mavlink_mount_configure_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -422,11 +422,11 @@ static void mavlink_test_mount_control(uint8_t system_id, uint8_t component_id, uint16_t i; mavlink_mount_control_t packet_in = { 963497464, - 963497672, - 963497880, - 41, - 108, - 175, + }963497672, + }963497880, + }41, + }108, + }175, }; mavlink_mount_control_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -475,10 +475,10 @@ static void mavlink_test_mount_status(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_mount_status_t packet_in = { 963497464, - 963497672, - 963497880, - 41, - 108, + }963497672, + }963497880, + }41, + }108, }; mavlink_mount_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -526,11 +526,11 @@ static void mavlink_test_fence_point(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_fence_point_t packet_in = { 17.0, - 45.0, - 29, - 96, - 163, - 230, + }45.0, + }29, + }96, + }163, + }230, }; mavlink_fence_point_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -579,8 +579,8 @@ static void mavlink_test_fence_fetch_point(uint8_t system_id, uint8_t component_ uint16_t i; mavlink_fence_fetch_point_t packet_in = { 5, - 72, - 139, + }72, + }139, }; mavlink_fence_fetch_point_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -626,9 +626,9 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_fence_status_t packet_in = { 963497464, - 17443, - 151, - 218, + }17443, + }151, + }218, }; mavlink_fence_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -675,12 +675,12 @@ static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_m uint16_t i; mavlink_ahrs_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, }; mavlink_ahrs_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -730,16 +730,16 @@ static void mavlink_test_simstate(uint8_t system_id, uint8_t component_id, mavli uint16_t i; mavlink_simstate_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 963499336, - 963499544, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }963499336, + }963499544, }; mavlink_simstate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -793,7 +793,7 @@ static void mavlink_test_hwstatus(uint8_t system_id, uint8_t component_id, mavli uint16_t i; mavlink_hwstatus_t packet_in = { 17235, - 139, + }139, }; mavlink_hwstatus_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -838,12 +838,12 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_ uint16_t i; mavlink_radio_t packet_in = { 17235, - 17339, - 17, - 84, - 151, - 218, - 29, + }17339, + }17, + }84, + }151, + }218, + }29, }; mavlink_radio_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -893,14 +893,14 @@ static void mavlink_test_limits_status(uint8_t system_id, uint8_t component_id, uint16_t i; mavlink_limits_status_t packet_in = { 963497464, - 963497672, - 963497880, - 963498088, - 18067, - 187, - 254, - 65, - 132, + }963497672, + }963497880, + }963498088, + }18067, + }187, + }254, + }65, + }132, }; mavlink_limits_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -952,8 +952,8 @@ static void mavlink_test_wind(uint8_t system_id, uint8_t component_id, mavlink_m uint16_t i; mavlink_wind_t packet_in = { 17.0, - 45.0, - 73.0, + }45.0, + }73.0, }; mavlink_wind_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -999,8 +999,8 @@ static void mavlink_test_data16(uint8_t system_id, uint8_t component_id, mavlink uint16_t i; mavlink_data16_t packet_in = { 5, - 72, - { 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154 }, + }72, + }{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154 }, }; mavlink_data16_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1046,8 +1046,8 @@ static void mavlink_test_data32(uint8_t system_id, uint8_t component_id, mavlink uint16_t i; mavlink_data32_t packet_in = { 5, - 72, - { 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170 }, + }72, + }{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170 }, }; mavlink_data32_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1093,8 +1093,8 @@ static void mavlink_test_data64(uint8_t system_id, uint8_t component_id, mavlink uint16_t i; mavlink_data64_t packet_in = { 5, - 72, - { 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 }, + }72, + }{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 }, }; mavlink_data64_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1140,8 +1140,8 @@ static void mavlink_test_data96(uint8_t system_id, uint8_t component_id, mavlink uint16_t i; mavlink_data96_t packet_in = { 5, - 72, - { 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234 }, + }72, + }{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234 }, }; mavlink_data96_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1187,7 +1187,7 @@ static void mavlink_test_rangefinder(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_rangefinder_t packet_in = { 17.0, - 45.0, + }45.0, }; mavlink_rangefinder_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1232,17 +1232,17 @@ static void mavlink_test_airspeed_autocal(uint8_t system_id, uint8_t component_i uint16_t i; mavlink_airspeed_autocal_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 269.0, - 297.0, - 325.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }269.0, + }297.0, + }325.0, }; mavlink_airspeed_autocal_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1290,6 +1290,114 @@ static void mavlink_test_airspeed_autocal(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_rally_point(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_rally_point_t packet_in = { + 963497464, + }963497672, + }17651, + }17755, + }17859, + }175, + }242, + }53, + }120, + }187, + }; + mavlink_rally_point_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.lat = packet_in.lat; + packet1.lng = packet_in.lng; + packet1.alt = packet_in.alt; + packet1.break_alt = packet_in.break_alt; + packet1.land_dir = packet_in.land_dir; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.idx = packet_in.idx; + packet1.count = packet_in.count; + packet1.flags = packet_in.flags; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rally_point_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rally_point_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rally_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags ); + mavlink_msg_rally_point_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rally_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags ); + mavlink_msg_rally_point_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 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_aq_telemetry_f.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // AUTOQUAD_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink.h deleted file mode 100644 index 3f80c9a41e..0000000000 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from autoquad.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "autoquad.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h deleted file mode 100644 index ed7c86bcb8..0000000000 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ /dev/null @@ -1,617 +0,0 @@ -// MESSAGE AQ_TELEMETRY_F PACKING - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F 150 - -typedef struct __mavlink_aq_telemetry_f_t -{ - float value1; ///< value1 - float value2; ///< value2 - float value3; ///< value3 - float value4; ///< value4 - float value5; ///< value5 - float value6; ///< value6 - float value7; ///< value7 - float value8; ///< value8 - float value9; ///< value9 - float value10; ///< value10 - float value11; ///< value11 - float value12; ///< value12 - float value13; ///< value13 - float value14; ///< value14 - float value15; ///< value15 - float value16; ///< value16 - float value17; ///< value17 - float value18; ///< value18 - float value19; ///< value19 - float value20; ///< value20 - uint16_t Index; ///< Index of message -} mavlink_aq_telemetry_f_t; - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN 82 -#define MAVLINK_MSG_ID_150_LEN 82 - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC 241 -#define MAVLINK_MSG_ID_150_CRC 241 - - - -#define MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F { \ - "AQ_TELEMETRY_F", \ - 21, \ - { { "value1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_aq_telemetry_f_t, value1) }, \ - { "value2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_aq_telemetry_f_t, value2) }, \ - { "value3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_aq_telemetry_f_t, value3) }, \ - { "value4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_aq_telemetry_f_t, value4) }, \ - { "value5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_aq_telemetry_f_t, value5) }, \ - { "value6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_aq_telemetry_f_t, value6) }, \ - { "value7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_aq_telemetry_f_t, value7) }, \ - { "value8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_aq_telemetry_f_t, value8) }, \ - { "value9", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_aq_telemetry_f_t, value9) }, \ - { "value10", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_aq_telemetry_f_t, value10) }, \ - { "value11", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_aq_telemetry_f_t, value11) }, \ - { "value12", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_aq_telemetry_f_t, value12) }, \ - { "value13", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_aq_telemetry_f_t, value13) }, \ - { "value14", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_aq_telemetry_f_t, value14) }, \ - { "value15", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_aq_telemetry_f_t, value15) }, \ - { "value16", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_aq_telemetry_f_t, value16) }, \ - { "value17", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_aq_telemetry_f_t, value17) }, \ - { "value18", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_aq_telemetry_f_t, value18) }, \ - { "value19", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_aq_telemetry_f_t, value19) }, \ - { "value20", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_aq_telemetry_f_t, value20) }, \ - { "Index", NULL, MAVLINK_TYPE_UINT16_T, 0, 80, offsetof(mavlink_aq_telemetry_f_t, Index) }, \ - } \ -} - - -/** - * @brief Pack a aq_telemetry_f 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 Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} - -/** - * @brief Pack a aq_telemetry_f 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 will be sent over - * @param msg The MAVLink message to compress the data into - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t Index,float value1,float value2,float value3,float value4,float value5,float value6,float value7,float value8,float value9,float value10,float value11,float value12,float value13,float value14,float value15,float value16,float value17,float value18,float value19,float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} - -/** - * @brief Encode a aq_telemetry_f struct - * - * @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 aq_telemetry_f C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ - return mavlink_msg_aq_telemetry_f_pack(system_id, component_id, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); -} - -/** - * @brief Encode a aq_telemetry_f struct 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 will be sent over - * @param msg The MAVLink message to compress the data into - * @param aq_telemetry_f C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ - return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); -} - -/** - * @brief Send a aq_telemetry_f message - * @param chan MAVLink channel to send the message - * - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_aq_telemetry_f_send(mavlink_channel_t chan, uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#endif -} - -#endif - -// MESSAGE AQ_TELEMETRY_F UNPACKING - - -/** - * @brief Get field Index from aq_telemetry_f message - * - * @return Index of message - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_get_Index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 80); -} - -/** - * @brief Get field value1 from aq_telemetry_f message - * - * @return value1 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field value2 from aq_telemetry_f message - * - * @return value2 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field value3 from aq_telemetry_f message - * - * @return value3 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field value4 from aq_telemetry_f message - * - * @return value4 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field value5 from aq_telemetry_f message - * - * @return value5 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field value6 from aq_telemetry_f message - * - * @return value6 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field value7 from aq_telemetry_f message - * - * @return value7 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field value8 from aq_telemetry_f message - * - * @return value8 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field value9 from aq_telemetry_f message - * - * @return value9 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value9(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field value10 from aq_telemetry_f message - * - * @return value10 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value10(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field value11 from aq_telemetry_f message - * - * @return value11 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value11(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field value12 from aq_telemetry_f message - * - * @return value12 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value12(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field value13 from aq_telemetry_f message - * - * @return value13 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value13(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field value14 from aq_telemetry_f message - * - * @return value14 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value14(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field value15 from aq_telemetry_f message - * - * @return value15 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value15(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field value16 from aq_telemetry_f message - * - * @return value16 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 60); -} - -/** - * @brief Get field value17 from aq_telemetry_f message - * - * @return value17 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value17(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 64); -} - -/** - * @brief Get field value18 from aq_telemetry_f message - * - * @return value18 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value18(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 68); -} - -/** - * @brief Get field value19 from aq_telemetry_f message - * - * @return value19 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value19(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 72); -} - -/** - * @brief Get field value20 from aq_telemetry_f message - * - * @return value20 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value20(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 76); -} - -/** - * @brief Decode a aq_telemetry_f message into a struct - * - * @param msg The message to decode - * @param aq_telemetry_f C-struct to decode the message contents into - */ -static inline void mavlink_msg_aq_telemetry_f_decode(const mavlink_message_t* msg, mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ -#if MAVLINK_NEED_BYTE_SWAP - aq_telemetry_f->value1 = mavlink_msg_aq_telemetry_f_get_value1(msg); - aq_telemetry_f->value2 = mavlink_msg_aq_telemetry_f_get_value2(msg); - aq_telemetry_f->value3 = mavlink_msg_aq_telemetry_f_get_value3(msg); - aq_telemetry_f->value4 = mavlink_msg_aq_telemetry_f_get_value4(msg); - aq_telemetry_f->value5 = mavlink_msg_aq_telemetry_f_get_value5(msg); - aq_telemetry_f->value6 = mavlink_msg_aq_telemetry_f_get_value6(msg); - aq_telemetry_f->value7 = mavlink_msg_aq_telemetry_f_get_value7(msg); - aq_telemetry_f->value8 = mavlink_msg_aq_telemetry_f_get_value8(msg); - aq_telemetry_f->value9 = mavlink_msg_aq_telemetry_f_get_value9(msg); - aq_telemetry_f->value10 = mavlink_msg_aq_telemetry_f_get_value10(msg); - aq_telemetry_f->value11 = mavlink_msg_aq_telemetry_f_get_value11(msg); - aq_telemetry_f->value12 = mavlink_msg_aq_telemetry_f_get_value12(msg); - aq_telemetry_f->value13 = mavlink_msg_aq_telemetry_f_get_value13(msg); - aq_telemetry_f->value14 = mavlink_msg_aq_telemetry_f_get_value14(msg); - aq_telemetry_f->value15 = mavlink_msg_aq_telemetry_f_get_value15(msg); - aq_telemetry_f->value16 = mavlink_msg_aq_telemetry_f_get_value16(msg); - aq_telemetry_f->value17 = mavlink_msg_aq_telemetry_f_get_value17(msg); - aq_telemetry_f->value18 = mavlink_msg_aq_telemetry_f_get_value18(msg); - aq_telemetry_f->value19 = mavlink_msg_aq_telemetry_f_get_value19(msg); - aq_telemetry_f->value20 = mavlink_msg_aq_telemetry_f_get_value20(msg); - aq_telemetry_f->Index = mavlink_msg_aq_telemetry_f_get_Index(msg); -#else - memcpy(aq_telemetry_f, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h b/mavlink/include/mavlink/v1.0/autoquad/testsuite.h deleted file mode 100644 index dbec869cea..0000000000 --- a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h +++ /dev/null @@ -1,118 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from autoquad.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef AUTOQUAD_TESTSUITE_H -#define AUTOQUAD_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_autoquad(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_test_common(system_id, component_id, last_msg); - mavlink_test_autoquad(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_aq_telemetry_f(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_aq_telemetry_f_t packet_in = { - 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 269.0, - 297.0, - 325.0, - 353.0, - 381.0, - 409.0, - 437.0, - 465.0, - 493.0, - 521.0, - 549.0, - 21395, - }; - mavlink_aq_telemetry_f_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.value1 = packet_in.value1; - packet1.value2 = packet_in.value2; - packet1.value3 = packet_in.value3; - packet1.value4 = packet_in.value4; - packet1.value5 = packet_in.value5; - packet1.value6 = packet_in.value6; - packet1.value7 = packet_in.value7; - packet1.value8 = packet_in.value8; - packet1.value9 = packet_in.value9; - packet1.value10 = packet_in.value10; - packet1.value11 = packet_in.value11; - packet1.value12 = packet_in.value12; - packet1.value13 = packet_in.value13; - packet1.value14 = packet_in.value14; - packet1.value15 = packet_in.value15; - packet1.value16 = packet_in.value16; - packet1.value17 = packet_in.value17; - packet1.value18 = packet_in.value18; - packet1.value19 = packet_in.value19; - packet1.value20 = packet_in.value20; - packet1.Index = packet_in.Index; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_pack(system_id, component_id, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); - mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); - mavlink_msg_aq_telemetry_f_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; iaccu_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); + 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->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); } /** @@ -176,7 +192,7 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) { - return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, 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); + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, 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->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); } /** @@ -191,23 +207,27 @@ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, * @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 current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate * @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) +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, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; - _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_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_uint16_t(buf, 8, voltage_cell_1); + _mav_put_uint16_t(buf, 10, voltage_cell_2); + _mav_put_uint16_t(buf, 12, voltage_cell_3); + _mav_put_uint16_t(buf, 14, voltage_cell_4); + _mav_put_uint16_t(buf, 16, voltage_cell_5); + _mav_put_uint16_t(buf, 18, voltage_cell_6); + _mav_put_int16_t(buf, 20, current_battery); + _mav_put_uint8_t(buf, 22, accu_id); + _mav_put_int8_t(buf, 23, battery_remaining); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); @@ -216,6 +236,8 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 #endif #else mavlink_battery_status_t packet; + packet.current_consumed = current_consumed; + packet.energy_consumed = energy_consumed; packet.voltage_cell_1 = voltage_cell_1; packet.voltage_cell_2 = voltage_cell_2; packet.voltage_cell_3 = voltage_cell_3; @@ -246,7 +268,7 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 14); + return _MAV_RETURN_uint8_t(msg, 22); } /** @@ -256,7 +278,7 @@ static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_messa */ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -266,7 +288,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavli */ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 2); + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -276,7 +298,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavli */ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -286,7 +308,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavli */ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -296,7 +318,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavli */ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -306,7 +328,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavli */ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 18); } /** @@ -316,7 +338,27 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavli */ static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg) { - return _MAV_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field current_consumed from battery_status message + * + * @return Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + */ +static inline int32_t mavlink_msg_battery_status_get_current_consumed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field energy_consumed from battery_status message + * + * @return Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate + */ +static inline int32_t mavlink_msg_battery_status_get_energy_consumed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -326,7 +368,7 @@ static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavli */ static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg) { - return _MAV_RETURN_int8_t(msg, 15); + return _MAV_RETURN_int8_t(msg, 23); } /** @@ -338,6 +380,8 @@ static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavl 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->current_consumed = mavlink_msg_battery_status_get_current_consumed(msg); + battery_status->energy_consumed = mavlink_msg_battery_status_get_energy_consumed(msg); 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); diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index 3054d4fdab..a105f8cdad 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -9,7 +9,7 @@ typedef struct __mavlink_gps_raw_int_t int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. @@ -53,7 +53,7 @@ typedef struct __mavlink_gps_raw_int_t * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 @@ -112,7 +112,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 @@ -197,7 +197,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 @@ -313,7 +313,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* /** * @brief Get field epv from gps_raw_int message * - * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h index 36a551872b..91aec5b08c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h @@ -9,7 +9,7 @@ typedef struct __mavlink_hil_gps_t int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 int16_t vn; ///< GPS velocity in cm/s in NORTH direction in earth-fixed NED frame int16_t ve; ///< GPS velocity in cm/s in EAST direction in earth-fixed NED frame @@ -59,7 +59,7 @@ typedef struct __mavlink_hil_gps_t * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame @@ -127,7 +127,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame @@ -221,7 +221,7 @@ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_ * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame @@ -346,7 +346,7 @@ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) /** * @brief Get field epv from hil_gps message * - * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 */ static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h new file mode 100644 index 0000000000..1cf5d15e48 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h @@ -0,0 +1,237 @@ +// MESSAGE LOG_DATA PACKING + +#define MAVLINK_MSG_ID_LOG_DATA 120 + +typedef struct __mavlink_log_data_t +{ + uint32_t ofs; ///< Offset into the log + uint16_t id; ///< Log id (from LOG_ENTRY reply) + uint8_t count; ///< Number of bytes (zero for end of log) + uint8_t data[90]; ///< log data +} mavlink_log_data_t; + +#define MAVLINK_MSG_ID_LOG_DATA_LEN 97 +#define MAVLINK_MSG_ID_120_LEN 97 + +#define MAVLINK_MSG_ID_LOG_DATA_CRC 134 +#define MAVLINK_MSG_ID_120_CRC 134 + +#define MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN 90 + +#define MAVLINK_MESSAGE_INFO_LOG_DATA { \ + "LOG_DATA", \ + 4, \ + { { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_data_t, ofs) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_log_data_t, id) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_log_data_t, count) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 90, 7, offsetof(mavlink_log_data_t, data) }, \ + } \ +} + + +/** + * @brief Pack a log_data 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 id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes (zero for end of log) + * @param data log data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#else + mavlink_log_data_t packet; + packet.ofs = ofs; + packet.id = id; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +} + +/** + * @brief Pack a log_data 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes (zero for end of log) + * @param data log data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t id,uint32_t ofs,uint8_t count,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#else + mavlink_log_data_t packet; + packet.ofs = ofs; + packet.id = id; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +} + +/** + * @brief Encode a log_data struct + * + * @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 log_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_data_t* log_data) +{ + return mavlink_msg_log_data_pack(system_id, component_id, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); +} + +/** + * @brief Encode a log_data struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_data_t* log_data) +{ + return mavlink_msg_log_data_pack_chan(system_id, component_id, chan, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); +} + +/** + * @brief Send a log_data message + * @param chan MAVLink channel to send the message + * + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes (zero for end of log) + * @param data log data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#else + mavlink_log_data_t packet; + packet.ofs = ofs; + packet.id = id; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#endif +} + +#endif + +// MESSAGE LOG_DATA UNPACKING + + +/** + * @brief Get field id from log_data message + * + * @return Log id (from LOG_ENTRY reply) + */ +static inline uint16_t mavlink_msg_log_data_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field ofs from log_data message + * + * @return Offset into the log + */ +static inline uint32_t mavlink_msg_log_data_get_ofs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from log_data message + * + * @return Number of bytes (zero for end of log) + */ +static inline uint8_t mavlink_msg_log_data_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field data from log_data message + * + * @return log data + */ +static inline uint16_t mavlink_msg_log_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 90, 7); +} + +/** + * @brief Decode a log_data message into a struct + * + * @param msg The message to decode + * @param log_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_data_decode(const mavlink_message_t* msg, mavlink_log_data_t* log_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_data->ofs = mavlink_msg_log_data_get_ofs(msg); + log_data->id = mavlink_msg_log_data_get_id(msg); + log_data->count = mavlink_msg_log_data_get_count(msg); + mavlink_msg_log_data_get_data(msg, log_data->data); +#else + memcpy(log_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h new file mode 100644 index 0000000000..681d8f07cd --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h @@ -0,0 +1,265 @@ +// MESSAGE LOG_ENTRY PACKING + +#define MAVLINK_MSG_ID_LOG_ENTRY 118 + +typedef struct __mavlink_log_entry_t +{ + uint32_t time_utc; ///< UTC timestamp of log in seconds since 1970, or 0 if not available + uint32_t size; ///< Size of the log (may be approximate) in bytes + uint16_t id; ///< Log id + uint16_t num_logs; ///< Total number of logs + uint16_t last_log_num; ///< High log number +} mavlink_log_entry_t; + +#define MAVLINK_MSG_ID_LOG_ENTRY_LEN 14 +#define MAVLINK_MSG_ID_118_LEN 14 + +#define MAVLINK_MSG_ID_LOG_ENTRY_CRC 56 +#define MAVLINK_MSG_ID_118_CRC 56 + + + +#define MAVLINK_MESSAGE_INFO_LOG_ENTRY { \ + "LOG_ENTRY", \ + 5, \ + { { "time_utc", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_entry_t, time_utc) }, \ + { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_entry_t, size) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_entry_t, id) }, \ + { "num_logs", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_log_entry_t, num_logs) }, \ + { "last_log_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_log_entry_t, last_log_num) }, \ + } \ +} + + +/** + * @brief Pack a log_entry 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 id Log id + * @param num_logs Total number of logs + * @param last_log_num High log number + * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available + * @param size Size of the log (may be approximate) in bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_entry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#else + mavlink_log_entry_t packet; + packet.time_utc = time_utc; + packet.size = size; + packet.id = id; + packet.num_logs = num_logs; + packet.last_log_num = last_log_num; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +} + +/** + * @brief Pack a log_entry 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Log id + * @param num_logs Total number of logs + * @param last_log_num High log number + * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available + * @param size Size of the log (may be approximate) in bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_entry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t id,uint16_t num_logs,uint16_t last_log_num,uint32_t time_utc,uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#else + mavlink_log_entry_t packet; + packet.time_utc = time_utc; + packet.size = size; + packet.id = id; + packet.num_logs = num_logs; + packet.last_log_num = last_log_num; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +} + +/** + * @brief Encode a log_entry struct + * + * @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 log_entry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_entry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry) +{ + return mavlink_msg_log_entry_pack(system_id, component_id, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); +} + +/** + * @brief Encode a log_entry struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_entry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_entry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry) +{ + return mavlink_msg_log_entry_pack_chan(system_id, component_id, chan, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); +} + +/** + * @brief Send a log_entry message + * @param chan MAVLink channel to send the message + * + * @param id Log id + * @param num_logs Total number of logs + * @param last_log_num High log number + * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available + * @param size Size of the log (may be approximate) in bytes + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#else + mavlink_log_entry_t packet; + packet.time_utc = time_utc; + packet.size = size; + packet.id = id; + packet.num_logs = num_logs; + packet.last_log_num = last_log_num; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#endif +} + +#endif + +// MESSAGE LOG_ENTRY UNPACKING + + +/** + * @brief Get field id from log_entry message + * + * @return Log id + */ +static inline uint16_t mavlink_msg_log_entry_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field num_logs from log_entry message + * + * @return Total number of logs + */ +static inline uint16_t mavlink_msg_log_entry_get_num_logs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field last_log_num from log_entry message + * + * @return High log number + */ +static inline uint16_t mavlink_msg_log_entry_get_last_log_num(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field time_utc from log_entry message + * + * @return UTC timestamp of log in seconds since 1970, or 0 if not available + */ +static inline uint32_t mavlink_msg_log_entry_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field size from log_entry message + * + * @return Size of the log (may be approximate) in bytes + */ +static inline uint32_t mavlink_msg_log_entry_get_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Decode a log_entry message into a struct + * + * @param msg The message to decode + * @param log_entry C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_entry_decode(const mavlink_message_t* msg, mavlink_log_entry_t* log_entry) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_entry->time_utc = mavlink_msg_log_entry_get_time_utc(msg); + log_entry->size = mavlink_msg_log_entry_get_size(msg); + log_entry->id = mavlink_msg_log_entry_get_id(msg); + log_entry->num_logs = mavlink_msg_log_entry_get_num_logs(msg); + log_entry->last_log_num = mavlink_msg_log_entry_get_last_log_num(msg); +#else + memcpy(log_entry, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h new file mode 100644 index 0000000000..feafeaf164 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h @@ -0,0 +1,199 @@ +// MESSAGE LOG_ERASE PACKING + +#define MAVLINK_MSG_ID_LOG_ERASE 121 + +typedef struct __mavlink_log_erase_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_erase_t; + +#define MAVLINK_MSG_ID_LOG_ERASE_LEN 2 +#define MAVLINK_MSG_ID_121_LEN 2 + +#define MAVLINK_MSG_ID_LOG_ERASE_CRC 237 +#define MAVLINK_MSG_ID_121_CRC 237 + + + +#define MAVLINK_MESSAGE_INFO_LOG_ERASE { \ + "LOG_ERASE", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_erase_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_erase_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_erase 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 target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_erase_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#else + mavlink_log_erase_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +} + +/** + * @brief Pack a log_erase 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_erase_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#else + mavlink_log_erase_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +} + +/** + * @brief Encode a log_erase struct + * + * @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 log_erase C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_erase_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) +{ + return mavlink_msg_log_erase_pack(system_id, component_id, msg, log_erase->target_system, log_erase->target_component); +} + +/** + * @brief Encode a log_erase struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_erase C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_erase_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) +{ + return mavlink_msg_log_erase_pack_chan(system_id, component_id, chan, msg, log_erase->target_system, log_erase->target_component); +} + +/** + * @brief Send a log_erase message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#else + mavlink_log_erase_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#endif +} + +#endif + +// MESSAGE LOG_ERASE UNPACKING + + +/** + * @brief Get field target_system from log_erase message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_erase_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from log_erase message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_erase_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a log_erase message into a struct + * + * @param msg The message to decode + * @param log_erase C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_erase_decode(const mavlink_message_t* msg, mavlink_log_erase_t* log_erase) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_erase->target_system = mavlink_msg_log_erase_get_target_system(msg); + log_erase->target_component = mavlink_msg_log_erase_get_target_component(msg); +#else + memcpy(log_erase, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h new file mode 100644 index 0000000000..5be9eea471 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h @@ -0,0 +1,265 @@ +// MESSAGE LOG_REQUEST_DATA PACKING + +#define MAVLINK_MSG_ID_LOG_REQUEST_DATA 119 + +typedef struct __mavlink_log_request_data_t +{ + uint32_t ofs; ///< Offset into the log + uint32_t count; ///< Number of bytes + uint16_t id; ///< Log id (from LOG_ENTRY reply) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_request_data_t; + +#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN 12 +#define MAVLINK_MSG_ID_119_LEN 12 + +#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC 116 +#define MAVLINK_MSG_ID_119_CRC 116 + + + +#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA { \ + "LOG_REQUEST_DATA", \ + 5, \ + { { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_request_data_t, ofs) }, \ + { "count", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_request_data_t, count) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_request_data_t, id) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_log_request_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_log_request_data_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_request_data 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 target_component Component ID + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#else + mavlink_log_request_data_t packet; + packet.ofs = ofs; + packet.count = count; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +} + +/** + * @brief Pack a log_request_data 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t id,uint32_t ofs,uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#else + mavlink_log_request_data_t packet; + packet.ofs = ofs; + packet.count = count; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +} + +/** + * @brief Encode a log_request_data struct + * + * @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 log_request_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data) +{ + return mavlink_msg_log_request_data_pack(system_id, component_id, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); +} + +/** + * @brief Encode a log_request_data struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_request_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data) +{ + return mavlink_msg_log_request_data_pack_chan(system_id, component_id, chan, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); +} + +/** + * @brief Send a log_request_data message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#else + mavlink_log_request_data_t packet; + packet.ofs = ofs; + packet.count = count; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#endif +} + +#endif + +// MESSAGE LOG_REQUEST_DATA UNPACKING + + +/** + * @brief Get field target_system from log_request_data message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_request_data_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field target_component from log_request_data message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_request_data_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field id from log_request_data message + * + * @return Log id (from LOG_ENTRY reply) + */ +static inline uint16_t mavlink_msg_log_request_data_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field ofs from log_request_data message + * + * @return Offset into the log + */ +static inline uint32_t mavlink_msg_log_request_data_get_ofs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from log_request_data message + * + * @return Number of bytes + */ +static inline uint32_t mavlink_msg_log_request_data_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Decode a log_request_data message into a struct + * + * @param msg The message to decode + * @param log_request_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_request_data_decode(const mavlink_message_t* msg, mavlink_log_request_data_t* log_request_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_request_data->ofs = mavlink_msg_log_request_data_get_ofs(msg); + log_request_data->count = mavlink_msg_log_request_data_get_count(msg); + log_request_data->id = mavlink_msg_log_request_data_get_id(msg); + log_request_data->target_system = mavlink_msg_log_request_data_get_target_system(msg); + log_request_data->target_component = mavlink_msg_log_request_data_get_target_component(msg); +#else + memcpy(log_request_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h new file mode 100644 index 0000000000..48a5a03b4c --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h @@ -0,0 +1,199 @@ +// MESSAGE LOG_REQUEST_END PACKING + +#define MAVLINK_MSG_ID_LOG_REQUEST_END 122 + +typedef struct __mavlink_log_request_end_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_request_end_t; + +#define MAVLINK_MSG_ID_LOG_REQUEST_END_LEN 2 +#define MAVLINK_MSG_ID_122_LEN 2 + +#define MAVLINK_MSG_ID_LOG_REQUEST_END_CRC 203 +#define MAVLINK_MSG_ID_122_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_END { \ + "LOG_REQUEST_END", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_request_end_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_request_end_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_request_end 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 target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_end_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#else + mavlink_log_request_end_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +} + +/** + * @brief Pack a log_request_end 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_end_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#else + mavlink_log_request_end_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +} + +/** + * @brief Encode a log_request_end struct + * + * @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 log_request_end C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_end_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end) +{ + return mavlink_msg_log_request_end_pack(system_id, component_id, msg, log_request_end->target_system, log_request_end->target_component); +} + +/** + * @brief Encode a log_request_end struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_request_end C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_end_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end) +{ + return mavlink_msg_log_request_end_pack_chan(system_id, component_id, chan, msg, log_request_end->target_system, log_request_end->target_component); +} + +/** + * @brief Send a log_request_end message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#else + mavlink_log_request_end_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#endif +} + +#endif + +// MESSAGE LOG_REQUEST_END UNPACKING + + +/** + * @brief Get field target_system from log_request_end message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_request_end_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from log_request_end message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_request_end_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a log_request_end message into a struct + * + * @param msg The message to decode + * @param log_request_end C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_request_end_decode(const mavlink_message_t* msg, mavlink_log_request_end_t* log_request_end) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_request_end->target_system = mavlink_msg_log_request_end_get_target_system(msg); + log_request_end->target_component = mavlink_msg_log_request_end_get_target_component(msg); +#else + memcpy(log_request_end, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h new file mode 100644 index 0000000000..7a5b25c171 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h @@ -0,0 +1,243 @@ +// MESSAGE LOG_REQUEST_LIST PACKING + +#define MAVLINK_MSG_ID_LOG_REQUEST_LIST 117 + +typedef struct __mavlink_log_request_list_t +{ + uint16_t start; ///< First log id (0 for first available) + uint16_t end; ///< Last log id (0xffff for last available) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_request_list_t; + +#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN 6 +#define MAVLINK_MSG_ID_117_LEN 6 + +#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC 128 +#define MAVLINK_MSG_ID_117_CRC 128 + + + +#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST { \ + "LOG_REQUEST_LIST", \ + 4, \ + { { "start", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_log_request_list_t, start) }, \ + { "end", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_log_request_list_t, end) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_log_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_log_request_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_request_list 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 target_component Component ID + * @param start First log id (0 for first available) + * @param end Last log id (0xffff for last available) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#else + mavlink_log_request_list_t packet; + packet.start = start; + packet.end = end; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Pack a log_request_list 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param start First log id (0 for first available) + * @param end Last log id (0xffff for last available) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t start,uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#else + mavlink_log_request_list_t packet; + packet.start = start; + packet.end = end; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Encode a log_request_list struct + * + * @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 log_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list) +{ + return mavlink_msg_log_request_list_pack(system_id, component_id, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); +} + +/** + * @brief Encode a log_request_list struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list) +{ + return mavlink_msg_log_request_list_pack_chan(system_id, component_id, chan, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); +} + +/** + * @brief Send a log_request_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param start First log id (0 for first available) + * @param end Last log id (0xffff for last available) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#else + mavlink_log_request_list_t packet; + packet.start = start; + packet.end = end; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#endif +} + +#endif + +// MESSAGE LOG_REQUEST_LIST UNPACKING + + +/** + * @brief Get field target_system from log_request_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_request_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from log_request_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_request_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field start from log_request_list message + * + * @return First log id (0 for first available) + */ +static inline uint16_t mavlink_msg_log_request_list_get_start(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field end from log_request_list message + * + * @return Last log id (0xffff for last available) + */ +static inline uint16_t mavlink_msg_log_request_list_get_end(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a log_request_list message into a struct + * + * @param msg The message to decode + * @param log_request_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_request_list_decode(const mavlink_message_t* msg, mavlink_log_request_list_t* log_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_request_list->start = mavlink_msg_log_request_list_get_start(msg); + log_request_list->end = mavlink_msg_log_request_list_get_end(msg); + log_request_list->target_system = mavlink_msg_log_request_list_get_target_system(msg); + log_request_list->target_component = mavlink_msg_log_request_list_get_target_component(msg); +#else + memcpy(log_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h index 634f80c379..a8d60eca7e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h @@ -4,13 +4,13 @@ typedef struct __mavlink_mission_item_t { - float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters - float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH + float param1; ///< PARAM1, see MAV_CMD enum + float param2; ///< PARAM2, see MAV_CMD enum + float param3; ///< PARAM3, see MAV_CMD enum + float param4; ///< PARAM4, see MAV_CMD enum float x; ///< PARAM5 / local: x position, global: latitude float y; ///< PARAM6 / y position: global: longitude - float z; ///< PARAM7 / z position: global: altitude + float z; ///< PARAM7 / z position: global: altitude (relative or absolute, depending on frame. uint16_t seq; ///< Sequence uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs uint8_t target_system; ///< System ID @@ -62,13 +62,13 @@ typedef struct __mavlink_mission_item_t * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs * @param current false:0, true:1 * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum * @param x PARAM5 / local: x position, global: latitude * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude + * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -133,13 +133,13 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs * @param current false:0, true:1 * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum * @param x PARAM5 / local: x position, global: latitude * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude + * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -230,13 +230,13 @@ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, u * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs * @param current false:0, true:1 * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum * @param x PARAM5 / local: x position, global: latitude * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude + * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -367,7 +367,7 @@ static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_me /** * @brief Get field param1 from mission_item message * - * @return PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters + * @return PARAM1, see MAV_CMD enum */ static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t* msg) { @@ -377,7 +377,7 @@ static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t* /** * @brief Get field param2 from mission_item message * - * @return PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + * @return PARAM2, see MAV_CMD enum */ static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t* msg) { @@ -387,7 +387,7 @@ static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t* /** * @brief Get field param3 from mission_item message * - * @return PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + * @return PARAM3, see MAV_CMD enum */ static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t* msg) { @@ -397,7 +397,7 @@ static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t* /** * @brief Get field param4 from mission_item message * - * @return PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH + * @return PARAM4, see MAV_CMD enum */ static inline float mavlink_msg_mission_item_get_param4(const mavlink_message_t* msg) { @@ -427,7 +427,7 @@ static inline float mavlink_msg_mission_item_get_y(const mavlink_message_t* msg) /** * @brief Get field z from mission_item message * - * @return PARAM7 / z position: global: altitude + * @return PARAM7 / z position: global: altitude (relative or absolute, depending on frame. */ static inline float mavlink_msg_mission_item_get_z(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h new file mode 100644 index 0000000000..ea4dbbf81d --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h @@ -0,0 +1,375 @@ +// MESSAGE SCALED_IMU2 PACKING + +#define MAVLINK_MSG_ID_SCALED_IMU2 116 + +typedef struct __mavlink_scaled_imu2_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + int16_t xgyro; ///< Angular speed around X axis (millirad /sec) + int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) + int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) + int16_t xmag; ///< X Magnetic field (milli tesla) + int16_t ymag; ///< Y Magnetic field (milli tesla) + int16_t zmag; ///< Z Magnetic field (milli tesla) +} mavlink_scaled_imu2_t; + +#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 22 +#define MAVLINK_MSG_ID_116_LEN 22 + +#define MAVLINK_MSG_ID_SCALED_IMU2_CRC 76 +#define MAVLINK_MSG_ID_116_CRC 76 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \ + "SCALED_IMU2", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu2_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu2_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu2_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu2_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \ + } \ +} + + +/** + * @brief Pack a scaled_imu2 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 time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#else + mavlink_scaled_imu2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +} + +/** + * @brief Pack a scaled_imu2 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#else + mavlink_scaled_imu2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +} + +/** + * @brief Encode a scaled_imu2 struct + * + * @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 scaled_imu2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) +{ + return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); +} + +/** + * @brief Encode a scaled_imu2 struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_imu2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) +{ + return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); +} + +/** + * @brief Send a scaled_imu2 message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#else + mavlink_scaled_imu2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#endif +} + +#endif + +// MESSAGE SCALED_IMU2 UNPACKING + + +/** + * @brief Get field time_boot_ms from scaled_imu2 message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_scaled_imu2_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field xacc from scaled_imu2 message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Get field yacc from scaled_imu2 message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 6); +} + +/** + * @brief Get field zacc from scaled_imu2 message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field xgyro from scaled_imu2 message + * + * @return Angular speed around X axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field ygyro from scaled_imu2 message + * + * @return Angular speed around Y axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field zgyro from scaled_imu2 message + * + * @return Angular speed around Z axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field xmag from scaled_imu2 message + * + * @return X Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field ymag from scaled_imu2 message + * + * @return Y Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field zmag from scaled_imu2 message + * + * @return Z Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Decode a scaled_imu2 message into a struct + * + * @param msg The message to decode + * @param scaled_imu2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_imu2_decode(const mavlink_message_t* msg, mavlink_scaled_imu2_t* scaled_imu2) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_imu2->time_boot_ms = mavlink_msg_scaled_imu2_get_time_boot_ms(msg); + scaled_imu2->xacc = mavlink_msg_scaled_imu2_get_xacc(msg); + scaled_imu2->yacc = mavlink_msg_scaled_imu2_get_yacc(msg); + scaled_imu2->zacc = mavlink_msg_scaled_imu2_get_zacc(msg); + scaled_imu2->xgyro = mavlink_msg_scaled_imu2_get_xgyro(msg); + scaled_imu2->ygyro = mavlink_msg_scaled_imu2_get_ygyro(msg); + scaled_imu2->zgyro = mavlink_msg_scaled_imu2_get_zgyro(msg); + scaled_imu2->xmag = mavlink_msg_scaled_imu2_get_xmag(msg); + scaled_imu2->ymag = mavlink_msg_scaled_imu2_get_ymag(msg); + scaled_imu2->zmag = mavlink_msg_scaled_imu2_get_zmag(msg); +#else + memcpy(scaled_imu2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h index 058c630ddd..101b366783 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h @@ -4,9 +4,9 @@ typedef struct __mavlink_sys_status_t { - uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt) int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current @@ -53,9 +53,9 @@ typedef struct __mavlink_sys_status_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current @@ -121,9 +121,9 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @param component_id ID of this component (e.g. 200 for IMU) * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into - * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current @@ -215,9 +215,9 @@ static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uin * @brief Send a sys_status message * @param chan MAVLink channel to send the message * - * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current @@ -286,7 +286,7 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t /** * @brief Get field onboard_control_sensors_present from sys_status message * - * @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR */ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg) { @@ -296,7 +296,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_presen /** * @brief Get field onboard_control_sensors_enabled from sys_status message * - * @return Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @return Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR */ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg) { @@ -306,7 +306,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enable /** * @brief Get field onboard_control_sensors_health from sys_status message * - * @return Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @return Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR */ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index 08dc664035..16250ab427 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -31,11 +31,11 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavl uint16_t i; mavlink_heartbeat_t packet_in = { 963497464, - 17, - 84, - 151, - 218, - 3, + }17, + }84, + }151, + }218, + }3, }; mavlink_heartbeat_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -84,18 +84,18 @@ static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mav uint16_t i; mavlink_sys_status_t packet_in = { 963497464, - 963497672, - 963497880, - 17859, - 17963, - 18067, - 18171, - 18275, - 18379, - 18483, - 18587, - 18691, - 223, + }963497672, + }963497880, + }17859, + }17963, + }18067, + }18171, + }18275, + }18379, + }18483, + }18587, + }18691, + }223, }; mavlink_sys_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -151,7 +151,7 @@ static void mavlink_test_system_time(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_system_time_t packet_in = { 93372036854775807ULL, - 963497880, + }963497880, }; mavlink_system_time_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -196,9 +196,9 @@ static void mavlink_test_ping(uint8_t system_id, uint8_t component_id, mavlink_m uint16_t i; mavlink_ping_t packet_in = { 93372036854775807ULL, - 963497880, - 41, - 108, + }963497880, + }41, + }108, }; mavlink_ping_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -245,9 +245,9 @@ static void mavlink_test_change_operator_control(uint8_t system_id, uint8_t comp uint16_t i; mavlink_change_operator_control_t packet_in = { 5, - 72, - 139, - "DEFGHIJKLMNOPQRSTUVWXYZA", + }72, + }139, + }"DEFGHIJKLMNOPQRSTUVWXYZA", }; mavlink_change_operator_control_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -294,8 +294,8 @@ static void mavlink_test_change_operator_control_ack(uint8_t system_id, uint8_t uint16_t i; mavlink_change_operator_control_ack_t packet_in = { 5, - 72, - 139, + }72, + }139, }; mavlink_change_operator_control_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -384,8 +384,8 @@ static void mavlink_test_set_mode(uint8_t system_id, uint8_t component_id, mavli uint16_t i; mavlink_set_mode_t packet_in = { 963497464, - 17, - 84, + }17, + }84, }; mavlink_set_mode_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -431,9 +431,9 @@ static void mavlink_test_param_request_read(uint8_t system_id, uint8_t component uint16_t i; mavlink_param_request_read_t packet_in = { 17235, - 139, - 206, - "EFGHIJKLMNOPQRS", + }139, + }206, + }"EFGHIJKLMNOPQRS", }; mavlink_param_request_read_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -480,7 +480,7 @@ static void mavlink_test_param_request_list(uint8_t system_id, uint8_t component uint16_t i; mavlink_param_request_list_t packet_in = { 5, - 72, + }72, }; mavlink_param_request_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -525,10 +525,10 @@ static void mavlink_test_param_value(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_param_value_t packet_in = { 17.0, - 17443, - 17547, - "IJKLMNOPQRSTUVW", - 77, + }17443, + }17547, + }"IJKLMNOPQRSTUVW", + }77, }; mavlink_param_value_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -576,10 +576,10 @@ static void mavlink_test_param_set(uint8_t system_id, uint8_t component_id, mavl uint16_t i; mavlink_param_set_t packet_in = { 17.0, - 17, - 84, - "GHIJKLMNOPQRSTU", - 199, + }17, + }84, + }"GHIJKLMNOPQRSTU", + }199, }; mavlink_param_set_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -627,15 +627,15 @@ static void mavlink_test_gps_raw_int(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_gps_raw_int_t packet_in = { 93372036854775807ULL, - 963497880, - 963498088, - 963498296, - 18275, - 18379, - 18483, - 18587, - 89, - 156, + }963497880, + }963498088, + }963498296, + }18275, + }18379, + }18483, + }18587, + }89, + }156, }; mavlink_gps_raw_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -688,11 +688,11 @@ static void mavlink_test_gps_status(uint8_t system_id, uint8_t component_id, mav uint16_t i; mavlink_gps_status_t packet_in = { 5, - { 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91 }, - { 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151 }, - { 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 }, - { 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }, - { 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75 }, + }{ 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91 }, + }{ 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151 }, + }{ 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 }, + }{ 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }, + }{ 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75 }, }; mavlink_gps_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -741,15 +741,15 @@ static void mavlink_test_scaled_imu(uint8_t system_id, uint8_t component_id, mav uint16_t i; mavlink_scaled_imu_t packet_in = { 963497464, - 17443, - 17547, - 17651, - 17755, - 17859, - 17963, - 18067, - 18171, - 18275, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }18275, }; mavlink_scaled_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -802,15 +802,15 @@ static void mavlink_test_raw_imu(uint8_t system_id, uint8_t component_id, mavlin uint16_t i; mavlink_raw_imu_t packet_in = { 93372036854775807ULL, - 17651, - 17755, - 17859, - 17963, - 18067, - 18171, - 18275, - 18379, - 18483, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }18275, + }18379, + }18483, }; mavlink_raw_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -863,10 +863,10 @@ static void mavlink_test_raw_pressure(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_raw_pressure_t packet_in = { 93372036854775807ULL, - 17651, - 17755, - 17859, - 17963, + }17651, + }17755, + }17859, + }17963, }; mavlink_raw_pressure_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -914,9 +914,9 @@ static void mavlink_test_scaled_pressure(uint8_t system_id, uint8_t component_id uint16_t i; mavlink_scaled_pressure_t packet_in = { 963497464, - 45.0, - 73.0, - 17859, + }45.0, + }73.0, + }17859, }; mavlink_scaled_pressure_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -963,12 +963,12 @@ static void mavlink_test_attitude(uint8_t system_id, uint8_t component_id, mavli uint16_t i; mavlink_attitude_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, }; mavlink_attitude_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1018,13 +1018,13 @@ static void mavlink_test_attitude_quaternion(uint8_t system_id, uint8_t componen uint16_t i; mavlink_attitude_quaternion_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, }; mavlink_attitude_quaternion_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1075,12 +1075,12 @@ static void mavlink_test_local_position_ned(uint8_t system_id, uint8_t component uint16_t i; mavlink_local_position_ned_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, }; mavlink_local_position_ned_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1130,14 +1130,14 @@ static void mavlink_test_global_position_int(uint8_t system_id, uint8_t componen uint16_t i; mavlink_global_position_int_t packet_in = { 963497464, - 963497672, - 963497880, - 963498088, - 963498296, - 18275, - 18379, - 18483, - 18587, + }963497672, + }963497880, + }963498088, + }963498296, + }18275, + }18379, + }18483, + }18587, }; mavlink_global_position_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1189,16 +1189,16 @@ static void mavlink_test_rc_channels_scaled(uint8_t system_id, uint8_t component uint16_t i; mavlink_rc_channels_scaled_t packet_in = { 963497464, - 17443, - 17547, - 17651, - 17755, - 17859, - 17963, - 18067, - 18171, - 65, - 132, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }65, + }132, }; mavlink_rc_channels_scaled_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1252,16 +1252,16 @@ static void mavlink_test_rc_channels_raw(uint8_t system_id, uint8_t component_id uint16_t i; mavlink_rc_channels_raw_t packet_in = { 963497464, - 17443, - 17547, - 17651, - 17755, - 17859, - 17963, - 18067, - 18171, - 65, - 132, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }65, + }132, }; mavlink_rc_channels_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1315,15 +1315,15 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i uint16_t i; mavlink_servo_output_raw_t packet_in = { 963497464, - 17443, - 17547, - 17651, - 17755, - 17859, - 17963, - 18067, - 18171, - 65, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }65, }; mavlink_servo_output_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1376,9 +1376,9 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t uint16_t i; mavlink_mission_request_partial_list_t packet_in = { 17235, - 17339, - 17, - 84, + }17339, + }17, + }84, }; mavlink_mission_request_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1425,9 +1425,9 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c uint16_t i; mavlink_mission_write_partial_list_t packet_in = { 17235, - 17339, - 17, - 84, + }17339, + }17, + }84, }; mavlink_mission_write_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1474,19 +1474,19 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_mission_item_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 18691, - 18795, - 101, - 168, - 235, - 46, - 113, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }18691, + }18795, + }101, + }168, + }235, + }46, + }113, }; mavlink_mission_item_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1543,8 +1543,8 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id uint16_t i; mavlink_mission_request_t packet_in = { 17235, - 139, - 206, + }139, + }206, }; mavlink_mission_request_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1590,8 +1590,8 @@ static void mavlink_test_mission_set_current(uint8_t system_id, uint8_t componen uint16_t i; mavlink_mission_set_current_t packet_in = { 17235, - 139, - 206, + }139, + }206, }; mavlink_mission_set_current_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1680,7 +1680,7 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone uint16_t i; mavlink_mission_request_list_t packet_in = { 5, - 72, + }72, }; mavlink_mission_request_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1725,8 +1725,8 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, uint16_t i; mavlink_mission_count_t packet_in = { 17235, - 139, - 206, + }139, + }206, }; mavlink_mission_count_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1772,7 +1772,7 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ uint16_t i; mavlink_mission_clear_all_t packet_in = { 5, - 72, + }72, }; mavlink_mission_clear_all_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1860,8 +1860,8 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_mission_ack_t packet_in = { 5, - 72, - 139, + }72, + }139, }; mavlink_mission_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1907,9 +1907,9 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon uint16_t i; mavlink_set_gps_global_origin_t packet_in = { 963497464, - 963497672, - 963497880, - 41, + }963497672, + }963497880, + }41, }; mavlink_set_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1956,8 +1956,8 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ uint16_t i; mavlink_gps_global_origin_t packet_in = { 963497464, - 963497672, - 963497880, + }963497672, + }963497880, }; mavlink_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2003,12 +2003,12 @@ static void mavlink_test_set_local_position_setpoint(uint8_t system_id, uint8_t uint16_t i; mavlink_set_local_position_setpoint_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 53, - 120, - 187, + }45.0, + }73.0, + }101.0, + }53, + }120, + }187, }; mavlink_set_local_position_setpoint_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2058,10 +2058,10 @@ static void mavlink_test_local_position_setpoint(uint8_t system_id, uint8_t comp uint16_t i; mavlink_local_position_setpoint_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 53, + }45.0, + }73.0, + }101.0, + }53, }; mavlink_local_position_setpoint_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2109,10 +2109,10 @@ static void mavlink_test_global_position_setpoint_int(uint8_t system_id, uint8_t uint16_t i; mavlink_global_position_setpoint_int_t packet_in = { 963497464, - 963497672, - 963497880, - 17859, - 175, + }963497672, + }963497880, + }17859, + }175, }; mavlink_global_position_setpoint_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2160,10 +2160,10 @@ static void mavlink_test_set_global_position_setpoint_int(uint8_t system_id, uin uint16_t i; mavlink_set_global_position_setpoint_int_t packet_in = { 963497464, - 963497672, - 963497880, - 17859, - 175, + }963497672, + }963497880, + }17859, + }175, }; mavlink_set_global_position_setpoint_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2211,14 +2211,14 @@ static void mavlink_test_safety_set_allowed_area(uint8_t system_id, uint8_t comp uint16_t i; mavlink_safety_set_allowed_area_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 77, - 144, - 211, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }77, + }144, + }211, }; mavlink_safety_set_allowed_area_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2270,12 +2270,12 @@ static void mavlink_test_safety_allowed_area(uint8_t system_id, uint8_t componen uint16_t i; mavlink_safety_allowed_area_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 77, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }77, }; mavlink_safety_allowed_area_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2325,11 +2325,11 @@ static void mavlink_test_set_roll_pitch_yaw_thrust(uint8_t system_id, uint8_t co uint16_t i; mavlink_set_roll_pitch_yaw_thrust_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 53, - 120, + }45.0, + }73.0, + }101.0, + }53, + }120, }; mavlink_set_roll_pitch_yaw_thrust_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2378,11 +2378,11 @@ static void mavlink_test_set_roll_pitch_yaw_speed_thrust(uint8_t system_id, uint uint16_t i; mavlink_set_roll_pitch_yaw_speed_thrust_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 53, - 120, + }45.0, + }73.0, + }101.0, + }53, + }120, }; mavlink_set_roll_pitch_yaw_speed_thrust_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2431,10 +2431,10 @@ static void mavlink_test_roll_pitch_yaw_thrust_setpoint(uint8_t system_id, uint8 uint16_t i; mavlink_roll_pitch_yaw_thrust_setpoint_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, + }45.0, + }73.0, + }101.0, + }129.0, }; mavlink_roll_pitch_yaw_thrust_setpoint_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2482,10 +2482,10 @@ static void mavlink_test_roll_pitch_yaw_speed_thrust_setpoint(uint8_t system_id, uint16_t i; mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, + }45.0, + }73.0, + }101.0, + }129.0, }; mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2533,10 +2533,10 @@ static void mavlink_test_set_quad_motors_setpoint(uint8_t system_id, uint8_t com uint16_t i; mavlink_set_quad_motors_setpoint_t packet_in = { 17235, - 17339, - 17443, - 17547, - 29, + }17339, + }17443, + }17547, + }29, }; mavlink_set_quad_motors_setpoint_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2584,11 +2584,11 @@ static void mavlink_test_set_quad_swarm_roll_pitch_yaw_thrust(uint8_t system_id, uint16_t i; mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet_in = { { 17235, 17236, 17237, 17238 }, - { 17651, 17652, 17653, 17654 }, - { 18067, 18068, 18069, 18070 }, - { 18483, 18484, 18485, 18486 }, - 101, - 168, + }{ 17651, 17652, 17653, 17654 }, + }{ 18067, 18068, 18069, 18070 }, + }{ 18483, 18484, 18485, 18486 }, + }101, + }168, }; mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2637,13 +2637,13 @@ static void mavlink_test_nav_controller_output(uint8_t system_id, uint8_t compon uint16_t i; mavlink_nav_controller_output_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 18275, - 18379, - 18483, + }45.0, + }73.0, + }101.0, + }129.0, + }18275, + }18379, + }18483, }; mavlink_nav_controller_output_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2694,14 +2694,14 @@ static void mavlink_test_set_quad_swarm_led_roll_pitch_yaw_thrust(uint8_t system uint16_t i; mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet_in = { { 17235, 17236, 17237, 17238 }, - { 17651, 17652, 17653, 17654 }, - { 18067, 18068, 18069, 18070 }, - { 18483, 18484, 18485, 18486 }, - 101, - 168, - { 235, 236, 237, 238 }, - { 247, 248, 249, 250 }, - { 3, 4, 5, 6 }, + }{ 17651, 17652, 17653, 17654 }, + }{ 18067, 18068, 18069, 18070 }, + }{ 18483, 18484, 18485, 18486 }, + }101, + }168, + }{ 235, 236, 237, 238 }, + }{ 247, 248, 249, 250 }, + }{ 3, 4, 5, 6 }, }; mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2753,14 +2753,14 @@ static void mavlink_test_state_correction(uint8_t system_id, uint8_t component_i uint16_t i; mavlink_state_correction_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, }; mavlink_state_correction_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2812,10 +2812,10 @@ static void mavlink_test_request_data_stream(uint8_t system_id, uint8_t componen uint16_t i; mavlink_request_data_stream_t packet_in = { 17235, - 139, - 206, - 17, - 84, + }139, + }206, + }17, + }84, }; mavlink_request_data_stream_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2863,8 +2863,8 @@ static void mavlink_test_data_stream(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_data_stream_t packet_in = { 17235, - 139, - 206, + }139, + }206, }; mavlink_data_stream_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2910,11 +2910,11 @@ static void mavlink_test_manual_control(uint8_t system_id, uint8_t component_id, uint16_t i; mavlink_manual_control_t packet_in = { 17235, - 17339, - 17443, - 17547, - 17651, - 163, + }17339, + }17443, + }17547, + }17651, + }163, }; mavlink_manual_control_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2963,15 +2963,15 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone uint16_t i; mavlink_rc_channels_override_t packet_in = { 17235, - 17339, - 17443, - 17547, - 17651, - 17755, - 17859, - 17963, - 53, - 120, + }17339, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }53, + }120, }; mavlink_rc_channels_override_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3024,11 +3024,11 @@ static void mavlink_test_vfr_hud(uint8_t system_id, uint8_t component_id, mavlin uint16_t i; mavlink_vfr_hud_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 18067, - 18171, + }45.0, + }73.0, + }101.0, + }18067, + }18171, }; mavlink_vfr_hud_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3077,16 +3077,16 @@ static void mavlink_test_command_long(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_command_long_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 18691, - 223, - 34, - 101, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }18691, + }223, + }34, + }101, }; mavlink_command_long_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3140,7 +3140,7 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_command_ack_t packet_in = { 17235, - 139, + }139, }; mavlink_command_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3185,10 +3185,10 @@ static void mavlink_test_roll_pitch_yaw_rates_thrust_setpoint(uint8_t system_id, uint16_t i; mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, + }45.0, + }73.0, + }101.0, + }129.0, }; mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3236,12 +3236,12 @@ static void mavlink_test_manual_setpoint(uint8_t system_id, uint8_t component_id uint16_t i; mavlink_manual_setpoint_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, - 65, - 132, + }45.0, + }73.0, + }101.0, + }129.0, + }65, + }132, }; mavlink_manual_setpoint_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3291,12 +3291,12 @@ static void mavlink_test_local_position_ned_system_global_offset(uint8_t system_ uint16_t i; mavlink_local_position_ned_system_global_offset_t packet_in = { 963497464, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, }; mavlink_local_position_ned_system_global_offset_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3346,21 +3346,21 @@ static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavl uint16_t i; mavlink_hil_state_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 963499128, - 963499336, - 963499544, - 19523, - 19627, - 19731, - 19835, - 19939, - 20043, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }963499128, + }963499336, + }963499544, + }19523, + }19627, + }19731, + }19835, + }19939, + }20043, }; mavlink_hil_state_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3419,16 +3419,16 @@ static void mavlink_test_hil_controls(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_hil_controls_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 269.0, - 125, - 192, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }269.0, + }125, + }192, }; mavlink_hil_controls_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3482,19 +3482,19 @@ static void mavlink_test_hil_rc_inputs_raw(uint8_t system_id, uint8_t component_ uint16_t i; mavlink_hil_rc_inputs_raw_t packet_in = { 93372036854775807ULL, - 17651, - 17755, - 17859, - 17963, - 18067, - 18171, - 18275, - 18379, - 18483, - 18587, - 18691, - 18795, - 101, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }18275, + }18379, + }18483, + }18587, + }18691, + }18795, + }101, }; mavlink_hil_rc_inputs_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3551,13 +3551,13 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_optical_flow_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 18275, - 18379, - 77, - 144, + }73.0, + }101.0, + }129.0, + }18275, + }18379, + }77, + }144, }; mavlink_optical_flow_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3608,12 +3608,12 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint uint16_t i; mavlink_global_vision_position_estimate_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, }; mavlink_global_vision_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3663,12 +3663,12 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com uint16_t i; mavlink_vision_position_estimate_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, }; mavlink_vision_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3718,9 +3718,9 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon uint16_t i; mavlink_vision_speed_estimate_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, + }73.0, + }101.0, + }129.0, }; mavlink_vision_speed_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3767,12 +3767,12 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp uint16_t i; mavlink_vicon_position_estimate_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, }; mavlink_vicon_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3822,20 +3822,20 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma uint16_t i; mavlink_highres_imu_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 269.0, - 297.0, - 325.0, - 353.0, - 381.0, - 409.0, - 20355, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }269.0, + }297.0, + }325.0, + }353.0, + }381.0, + }409.0, + }20355, }; mavlink_highres_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3893,11 +3893,11 @@ static void mavlink_test_omnidirectional_flow(uint8_t system_id, uint8_t compone uint16_t i; mavlink_omnidirectional_flow_t packet_in = { 93372036854775807ULL, - 73.0, - { 17859, 17860, 17861, 17862, 17863, 17864, 17865, 17866, 17867, 17868 }, - { 18899, 18900, 18901, 18902, 18903, 18904, 18905, 18906, 18907, 18908 }, - 161, - 228, + }73.0, + }{ 17859, 17860, 17861, 17862, 17863, 17864, 17865, 17866, 17867, 17868 }, + }{ 18899, 18900, 18901, 18902, 18903, 18904, 18905, 18906, 18907, 18908 }, + }161, + }228, }; mavlink_omnidirectional_flow_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3946,20 +3946,20 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav uint16_t i; mavlink_hil_sensor_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 269.0, - 297.0, - 325.0, - 353.0, - 381.0, - 409.0, - 963500584, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }269.0, + }297.0, + }325.0, + }353.0, + }381.0, + }409.0, + }963500584, }; mavlink_hil_sensor_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4017,26 +4017,26 @@ static void mavlink_test_sim_state(uint8_t system_id, uint8_t component_id, mavl uint16_t i; mavlink_sim_state_t packet_in = { 17.0, - 45.0, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - 241.0, - 269.0, - 297.0, - 325.0, - 353.0, - 381.0, - 409.0, - 437.0, - 465.0, - 493.0, - 521.0, - 549.0, - 577.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }269.0, + }297.0, + }325.0, + }353.0, + }381.0, + }409.0, + }437.0, + }465.0, + }493.0, + }521.0, + }549.0, + }577.0, }; mavlink_sim_state_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4100,12 +4100,12 @@ static void mavlink_test_radio_status(uint8_t system_id, uint8_t component_id, m uint16_t i; mavlink_radio_status_t packet_in = { 17235, - 17339, - 17, - 84, - 151, - 218, - 29, + }17339, + }17, + }84, + }151, + }218, + }29, }; mavlink_radio_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4155,10 +4155,10 @@ static void mavlink_test_file_transfer_start(uint8_t system_id, uint8_t componen uint16_t i; mavlink_file_transfer_start_t packet_in = { 93372036854775807ULL, - 963497880, - "MNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ", - 249, - 60, + }963497880, + }"MNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ", + }249, + }60, }; mavlink_file_transfer_start_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4206,8 +4206,8 @@ static void mavlink_test_file_transfer_dir_list(uint8_t system_id, uint8_t compo uint16_t i; mavlink_file_transfer_dir_list_t packet_in = { 93372036854775807ULL, - "IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLM", - 237, + }"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLM", + }237, }; mavlink_file_transfer_dir_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4253,7 +4253,7 @@ static void mavlink_test_file_transfer_res(uint8_t system_id, uint8_t component_ uint16_t i; mavlink_file_transfer_res_t packet_in = { 93372036854775807ULL, - 29, + }29, }; mavlink_file_transfer_res_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4298,18 +4298,18 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin uint16_t i; mavlink_hil_gps_t packet_in = { 93372036854775807ULL, - 963497880, - 963498088, - 963498296, - 18275, - 18379, - 18483, - 18587, - 18691, - 18795, - 18899, - 235, - 46, + }963497880, + }963498088, + }963498296, + }18275, + }18379, + }18483, + }18587, + }18691, + }18795, + }18899, + }235, + }46, }; mavlink_hil_gps_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4365,13 +4365,13 @@ static void mavlink_test_hil_optical_flow(uint8_t system_id, uint8_t component_i uint16_t i; mavlink_hil_optical_flow_t packet_in = { 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 18275, - 18379, - 77, - 144, + }73.0, + }101.0, + }129.0, + }18275, + }18379, + }77, + }144, }; mavlink_hil_optical_flow_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4422,21 +4422,21 @@ static void mavlink_test_hil_state_quaternion(uint8_t system_id, uint8_t compone uint16_t i; mavlink_hil_state_quaternion_t packet_in = { 93372036854775807ULL, - { 73.0, 74.0, 75.0, 76.0 }, - 185.0, - 213.0, - 241.0, - 963499336, - 963499544, - 963499752, - 19731, - 19835, - 19939, - 20043, - 20147, - 20251, - 20355, - 20459, + }{ 73.0, 74.0, 75.0, 76.0 }, + }185.0, + }213.0, + }241.0, + }963499336, + }963499544, + }963499752, + }19731, + }19835, + }19939, + }20043, + }20147, + }20251, + }20355, + }20459, }; mavlink_hil_state_quaternion_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4488,24 +4488,379 @@ static void mavlink_test_hil_state_quaternion(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_scaled_imu2(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_scaled_imu2_t packet_in = { + 963497464, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }18275, + }; + mavlink_scaled_imu2_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.xacc = packet_in.xacc; + packet1.yacc = packet_in.yacc; + packet1.zacc = packet_in.zacc; + packet1.xgyro = packet_in.xgyro; + packet1.ygyro = packet_in.ygyro; + packet1.zgyro = packet_in.zgyro; + packet1.xmag = packet_in.xmag; + packet1.ymag = packet_in.ymag; + packet1.zmag = packet_in.zmag; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_imu2_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_scaled_imu2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_imu2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_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: Mon, 16 Dec 2013 09:04:03 +0100 Subject: [PATCH 157/233] Re-introduced AQ MAVLink files since we have had them around before --- .../include/mavlink/v1.0/autoquad/autoquad.h | 123 ++++ .../include/mavlink/v1.0/autoquad/mavlink.h | 27 + .../autoquad/mavlink_msg_aq_telemetry_f.h | 617 ++++++++++++++++++ .../include/mavlink/v1.0/autoquad/testsuite.h | 118 ++++ .../include/mavlink/v1.0/autoquad/version.h | 12 + 5 files changed, 897 insertions(+) create mode 100644 mavlink/include/mavlink/v1.0/autoquad/autoquad.h create mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink.h create mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h create mode 100644 mavlink/include/mavlink/v1.0/autoquad/testsuite.h create mode 100644 mavlink/include/mavlink/v1.0/autoquad/version.h diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h new file mode 100644 index 0000000000..281aa8975c --- /dev/null +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -0,0 +1,123 @@ +/** @file + * @brief MAVLink comm protocol generated from autoquad.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef AUTOQUAD_H +#define AUTOQUAD_H + +#ifdef __cplusplus +extern "C" { +#endif + +// 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, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 222, 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, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 241, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_AQ_TELEMETRY_F, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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" + +#define MAVLINK_ENABLED_AUTOQUAD + +// ENUM DEFINITIONS + + +/** @brief Available operating modes/statuses for AutoQuad flight controller. + Bitmask up to 32 bits. Low side bits for base modes, high side for + additional active features/modifiers/constraints. */ +#ifndef HAVE_ENUM_AUTOQUAD_NAV_STATUS +#define HAVE_ENUM_AUTOQUAD_NAV_STATUS +enum AUTOQUAD_NAV_STATUS +{ + AQ_NAV_STATUS_INIT=0, /* System is initializing | */ + AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ + AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ + AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ + AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ + AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ + AQ_NAV_STATUS_CEILING=134217728, /* Ceiling altitude is set | */ + AQ_NAV_STATUS_HF_DYNAMIC=268435456, /* Heading-Free dynamic mode active | */ + AQ_NAV_STATUS_HF_LOCKED=536870912, /* Heading-Free locked mode active | */ + AQ_NAV_STATUS_RTH=1073741824, /* Automatic Return to Home is active | */ + AQ_NAV_STATUS_FAILSAFE=2147483648, /* System is in failsafe recovery mode | */ + AUTOQUAD_NAV_STATUS_ENUM_END=2147483649, /* | */ +}; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_CMD +#define HAVE_ENUM_MAV_CMD +enum MAV_CMD +{ + MAV_CMD_AQ_TELEMETRY=2, /* Start/stop AutoQuad telemetry values stream. |Start or stop (1 or 0)| Stream frequency in us| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_AQ_FOLLOW=3, /* Command AutoQuad to go to a particular place at a set speed. |Latitude| Lontitude| Altitude| Speed| Empty| Empty| Empty| */ + MAV_CMD_AQ_REQUEST_VERSION=4, /* Request AutoQuad firmware version number. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ +}; +#endif + +#include "../common/common.h" + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_aq_telemetry_f.h" + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // AUTOQUAD_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink.h new file mode 100644 index 0000000000..3f80c9a41e --- /dev/null +++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink.h @@ -0,0 +1,27 @@ +/** @file + * @brief MAVLink comm protocol built from autoquad.xml + * @see http://pixhawk.ethz.ch/software/mavlink + */ +#ifndef MAVLINK_H +#define MAVLINK_H + +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 +#endif + +#include "version.h" +#include "autoquad.h" + +#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h new file mode 100644 index 0000000000..ed7c86bcb8 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h @@ -0,0 +1,617 @@ +// MESSAGE AQ_TELEMETRY_F PACKING + +#define MAVLINK_MSG_ID_AQ_TELEMETRY_F 150 + +typedef struct __mavlink_aq_telemetry_f_t +{ + float value1; ///< value1 + float value2; ///< value2 + float value3; ///< value3 + float value4; ///< value4 + float value5; ///< value5 + float value6; ///< value6 + float value7; ///< value7 + float value8; ///< value8 + float value9; ///< value9 + float value10; ///< value10 + float value11; ///< value11 + float value12; ///< value12 + float value13; ///< value13 + float value14; ///< value14 + float value15; ///< value15 + float value16; ///< value16 + float value17; ///< value17 + float value18; ///< value18 + float value19; ///< value19 + float value20; ///< value20 + uint16_t Index; ///< Index of message +} mavlink_aq_telemetry_f_t; + +#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN 82 +#define MAVLINK_MSG_ID_150_LEN 82 + +#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC 241 +#define MAVLINK_MSG_ID_150_CRC 241 + + + +#define MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F { \ + "AQ_TELEMETRY_F", \ + 21, \ + { { "value1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_aq_telemetry_f_t, value1) }, \ + { "value2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_aq_telemetry_f_t, value2) }, \ + { "value3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_aq_telemetry_f_t, value3) }, \ + { "value4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_aq_telemetry_f_t, value4) }, \ + { "value5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_aq_telemetry_f_t, value5) }, \ + { "value6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_aq_telemetry_f_t, value6) }, \ + { "value7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_aq_telemetry_f_t, value7) }, \ + { "value8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_aq_telemetry_f_t, value8) }, \ + { "value9", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_aq_telemetry_f_t, value9) }, \ + { "value10", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_aq_telemetry_f_t, value10) }, \ + { "value11", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_aq_telemetry_f_t, value11) }, \ + { "value12", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_aq_telemetry_f_t, value12) }, \ + { "value13", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_aq_telemetry_f_t, value13) }, \ + { "value14", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_aq_telemetry_f_t, value14) }, \ + { "value15", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_aq_telemetry_f_t, value15) }, \ + { "value16", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_aq_telemetry_f_t, value16) }, \ + { "value17", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_aq_telemetry_f_t, value17) }, \ + { "value18", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_aq_telemetry_f_t, value18) }, \ + { "value19", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_aq_telemetry_f_t, value19) }, \ + { "value20", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_aq_telemetry_f_t, value20) }, \ + { "Index", NULL, MAVLINK_TYPE_UINT16_T, 0, 80, offsetof(mavlink_aq_telemetry_f_t, Index) }, \ + } \ +} + + +/** + * @brief Pack a aq_telemetry_f 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 Index Index of message + * @param value1 value1 + * @param value2 value2 + * @param value3 value3 + * @param value4 value4 + * @param value5 value5 + * @param value6 value6 + * @param value7 value7 + * @param value8 value8 + * @param value9 value9 + * @param value10 value10 + * @param value11 value11 + * @param value12 value12 + * @param value13 value13 + * @param value14 value14 + * @param value15 value15 + * @param value16 value16 + * @param value17 value17 + * @param value18 value18 + * @param value19 value19 + * @param value20 value20 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; + _mav_put_float(buf, 0, value1); + _mav_put_float(buf, 4, value2); + _mav_put_float(buf, 8, value3); + _mav_put_float(buf, 12, value4); + _mav_put_float(buf, 16, value5); + _mav_put_float(buf, 20, value6); + _mav_put_float(buf, 24, value7); + _mav_put_float(buf, 28, value8); + _mav_put_float(buf, 32, value9); + _mav_put_float(buf, 36, value10); + _mav_put_float(buf, 40, value11); + _mav_put_float(buf, 44, value12); + _mav_put_float(buf, 48, value13); + _mav_put_float(buf, 52, value14); + _mav_put_float(buf, 56, value15); + _mav_put_float(buf, 60, value16); + _mav_put_float(buf, 64, value17); + _mav_put_float(buf, 68, value18); + _mav_put_float(buf, 72, value19); + _mav_put_float(buf, 76, value20); + _mav_put_uint16_t(buf, 80, Index); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#else + mavlink_aq_telemetry_f_t packet; + packet.value1 = value1; + packet.value2 = value2; + packet.value3 = value3; + packet.value4 = value4; + packet.value5 = value5; + packet.value6 = value6; + packet.value7 = value7; + packet.value8 = value8; + packet.value9 = value9; + packet.value10 = value10; + packet.value11 = value11; + packet.value12 = value12; + packet.value13 = value13; + packet.value14 = value14; + packet.value15 = value15; + packet.value16 = value16; + packet.value17 = value17; + packet.value18 = value18; + packet.value19 = value19; + packet.value20 = value20; + packet.Index = Index; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +} + +/** + * @brief Pack a aq_telemetry_f 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param Index Index of message + * @param value1 value1 + * @param value2 value2 + * @param value3 value3 + * @param value4 value4 + * @param value5 value5 + * @param value6 value6 + * @param value7 value7 + * @param value8 value8 + * @param value9 value9 + * @param value10 value10 + * @param value11 value11 + * @param value12 value12 + * @param value13 value13 + * @param value14 value14 + * @param value15 value15 + * @param value16 value16 + * @param value17 value17 + * @param value18 value18 + * @param value19 value19 + * @param value20 value20 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t Index,float value1,float value2,float value3,float value4,float value5,float value6,float value7,float value8,float value9,float value10,float value11,float value12,float value13,float value14,float value15,float value16,float value17,float value18,float value19,float value20) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; + _mav_put_float(buf, 0, value1); + _mav_put_float(buf, 4, value2); + _mav_put_float(buf, 8, value3); + _mav_put_float(buf, 12, value4); + _mav_put_float(buf, 16, value5); + _mav_put_float(buf, 20, value6); + _mav_put_float(buf, 24, value7); + _mav_put_float(buf, 28, value8); + _mav_put_float(buf, 32, value9); + _mav_put_float(buf, 36, value10); + _mav_put_float(buf, 40, value11); + _mav_put_float(buf, 44, value12); + _mav_put_float(buf, 48, value13); + _mav_put_float(buf, 52, value14); + _mav_put_float(buf, 56, value15); + _mav_put_float(buf, 60, value16); + _mav_put_float(buf, 64, value17); + _mav_put_float(buf, 68, value18); + _mav_put_float(buf, 72, value19); + _mav_put_float(buf, 76, value20); + _mav_put_uint16_t(buf, 80, Index); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#else + mavlink_aq_telemetry_f_t packet; + packet.value1 = value1; + packet.value2 = value2; + packet.value3 = value3; + packet.value4 = value4; + packet.value5 = value5; + packet.value6 = value6; + packet.value7 = value7; + packet.value8 = value8; + packet.value9 = value9; + packet.value10 = value10; + packet.value11 = value11; + packet.value12 = value12; + packet.value13 = value13; + packet.value14 = value14; + packet.value15 = value15; + packet.value16 = value16; + packet.value17 = value17; + packet.value18 = value18; + packet.value19 = value19; + packet.value20 = value20; + packet.Index = Index; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +} + +/** + * @brief Encode a aq_telemetry_f struct + * + * @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 aq_telemetry_f C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) +{ + return mavlink_msg_aq_telemetry_f_pack(system_id, component_id, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); +} + +/** + * @brief Encode a aq_telemetry_f struct 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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param aq_telemetry_f C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) +{ + return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); +} + +/** + * @brief Send a aq_telemetry_f message + * @param chan MAVLink channel to send the message + * + * @param Index Index of message + * @param value1 value1 + * @param value2 value2 + * @param value3 value3 + * @param value4 value4 + * @param value5 value5 + * @param value6 value6 + * @param value7 value7 + * @param value8 value8 + * @param value9 value9 + * @param value10 value10 + * @param value11 value11 + * @param value12 value12 + * @param value13 value13 + * @param value14 value14 + * @param value15 value15 + * @param value16 value16 + * @param value17 value17 + * @param value18 value18 + * @param value19 value19 + * @param value20 value20 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_aq_telemetry_f_send(mavlink_channel_t chan, uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; + _mav_put_float(buf, 0, value1); + _mav_put_float(buf, 4, value2); + _mav_put_float(buf, 8, value3); + _mav_put_float(buf, 12, value4); + _mav_put_float(buf, 16, value5); + _mav_put_float(buf, 20, value6); + _mav_put_float(buf, 24, value7); + _mav_put_float(buf, 28, value8); + _mav_put_float(buf, 32, value9); + _mav_put_float(buf, 36, value10); + _mav_put_float(buf, 40, value11); + _mav_put_float(buf, 44, value12); + _mav_put_float(buf, 48, value13); + _mav_put_float(buf, 52, value14); + _mav_put_float(buf, 56, value15); + _mav_put_float(buf, 60, value16); + _mav_put_float(buf, 64, value17); + _mav_put_float(buf, 68, value18); + _mav_put_float(buf, 72, value19); + _mav_put_float(buf, 76, value20); + _mav_put_uint16_t(buf, 80, Index); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +#else + mavlink_aq_telemetry_f_t packet; + packet.value1 = value1; + packet.value2 = value2; + packet.value3 = value3; + packet.value4 = value4; + packet.value5 = value5; + packet.value6 = value6; + packet.value7 = value7; + packet.value8 = value8; + packet.value9 = value9; + packet.value10 = value10; + packet.value11 = value11; + packet.value12 = value12; + packet.value13 = value13; + packet.value14 = value14; + packet.value15 = value15; + packet.value16 = value16; + packet.value17 = value17; + packet.value18 = value18; + packet.value19 = value19; + packet.value20 = value20; + packet.Index = Index; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +#endif +} + +#endif + +// MESSAGE AQ_TELEMETRY_F UNPACKING + + +/** + * @brief Get field Index from aq_telemetry_f message + * + * @return Index of message + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_get_Index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 80); +} + +/** + * @brief Get field value1 from aq_telemetry_f message + * + * @return value1 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field value2 from aq_telemetry_f message + * + * @return value2 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field value3 from aq_telemetry_f message + * + * @return value3 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field value4 from aq_telemetry_f message + * + * @return value4 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field value5 from aq_telemetry_f message + * + * @return value5 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field value6 from aq_telemetry_f message + * + * @return value6 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field value7 from aq_telemetry_f message + * + * @return value7 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value7(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field value8 from aq_telemetry_f message + * + * @return value8 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value8(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field value9 from aq_telemetry_f message + * + * @return value9 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value9(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field value10 from aq_telemetry_f message + * + * @return value10 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value10(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field value11 from aq_telemetry_f message + * + * @return value11 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value11(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field value12 from aq_telemetry_f message + * + * @return value12 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value12(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field value13 from aq_telemetry_f message + * + * @return value13 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value13(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field value14 from aq_telemetry_f message + * + * @return value14 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value14(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field value15 from aq_telemetry_f message + * + * @return value15 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value15(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field value16 from aq_telemetry_f message + * + * @return value16 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value16(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 60); +} + +/** + * @brief Get field value17 from aq_telemetry_f message + * + * @return value17 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value17(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 64); +} + +/** + * @brief Get field value18 from aq_telemetry_f message + * + * @return value18 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value18(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 68); +} + +/** + * @brief Get field value19 from aq_telemetry_f message + * + * @return value19 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value19(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 72); +} + +/** + * @brief Get field value20 from aq_telemetry_f message + * + * @return value20 + */ +static inline float mavlink_msg_aq_telemetry_f_get_value20(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 76); +} + +/** + * @brief Decode a aq_telemetry_f message into a struct + * + * @param msg The message to decode + * @param aq_telemetry_f C-struct to decode the message contents into + */ +static inline void mavlink_msg_aq_telemetry_f_decode(const mavlink_message_t* msg, mavlink_aq_telemetry_f_t* aq_telemetry_f) +{ +#if MAVLINK_NEED_BYTE_SWAP + aq_telemetry_f->value1 = mavlink_msg_aq_telemetry_f_get_value1(msg); + aq_telemetry_f->value2 = mavlink_msg_aq_telemetry_f_get_value2(msg); + aq_telemetry_f->value3 = mavlink_msg_aq_telemetry_f_get_value3(msg); + aq_telemetry_f->value4 = mavlink_msg_aq_telemetry_f_get_value4(msg); + aq_telemetry_f->value5 = mavlink_msg_aq_telemetry_f_get_value5(msg); + aq_telemetry_f->value6 = mavlink_msg_aq_telemetry_f_get_value6(msg); + aq_telemetry_f->value7 = mavlink_msg_aq_telemetry_f_get_value7(msg); + aq_telemetry_f->value8 = mavlink_msg_aq_telemetry_f_get_value8(msg); + aq_telemetry_f->value9 = mavlink_msg_aq_telemetry_f_get_value9(msg); + aq_telemetry_f->value10 = mavlink_msg_aq_telemetry_f_get_value10(msg); + aq_telemetry_f->value11 = mavlink_msg_aq_telemetry_f_get_value11(msg); + aq_telemetry_f->value12 = mavlink_msg_aq_telemetry_f_get_value12(msg); + aq_telemetry_f->value13 = mavlink_msg_aq_telemetry_f_get_value13(msg); + aq_telemetry_f->value14 = mavlink_msg_aq_telemetry_f_get_value14(msg); + aq_telemetry_f->value15 = mavlink_msg_aq_telemetry_f_get_value15(msg); + aq_telemetry_f->value16 = mavlink_msg_aq_telemetry_f_get_value16(msg); + aq_telemetry_f->value17 = mavlink_msg_aq_telemetry_f_get_value17(msg); + aq_telemetry_f->value18 = mavlink_msg_aq_telemetry_f_get_value18(msg); + aq_telemetry_f->value19 = mavlink_msg_aq_telemetry_f_get_value19(msg); + aq_telemetry_f->value20 = mavlink_msg_aq_telemetry_f_get_value20(msg); + aq_telemetry_f->Index = mavlink_msg_aq_telemetry_f_get_Index(msg); +#else + memcpy(aq_telemetry_f, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h b/mavlink/include/mavlink/v1.0/autoquad/testsuite.h new file mode 100644 index 0000000000..4eafe7be5e --- /dev/null +++ b/mavlink/include/mavlink/v1.0/autoquad/testsuite.h @@ -0,0 +1,118 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from autoquad.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef AUTOQUAD_TESTSUITE_H +#define AUTOQUAD_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_autoquad(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_test_common(system_id, component_id, last_msg); + mavlink_test_autoquad(system_id, component_id, last_msg); +} +#endif + +#include "../common/testsuite.h" + + +static void mavlink_test_aq_telemetry_f(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_aq_telemetry_f_t packet_in = { + 17.0, + }45.0, + }73.0, + }101.0, + }129.0, + }157.0, + }185.0, + }213.0, + }241.0, + }269.0, + }297.0, + }325.0, + }353.0, + }381.0, + }409.0, + }437.0, + }465.0, + }493.0, + }521.0, + }549.0, + }21395, + }; + mavlink_aq_telemetry_f_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.value1 = packet_in.value1; + packet1.value2 = packet_in.value2; + packet1.value3 = packet_in.value3; + packet1.value4 = packet_in.value4; + packet1.value5 = packet_in.value5; + packet1.value6 = packet_in.value6; + packet1.value7 = packet_in.value7; + packet1.value8 = packet_in.value8; + packet1.value9 = packet_in.value9; + packet1.value10 = packet_in.value10; + packet1.value11 = packet_in.value11; + packet1.value12 = packet_in.value12; + packet1.value13 = packet_in.value13; + packet1.value14 = packet_in.value14; + packet1.value15 = packet_in.value15; + packet1.value16 = packet_in.value16; + packet1.value17 = packet_in.value17; + packet1.value18 = packet_in.value18; + packet1.value19 = packet_in.value19; + packet1.value20 = packet_in.value20; + packet1.Index = packet_in.Index; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_aq_telemetry_f_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_aq_telemetry_f_pack(system_id, component_id, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); + mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); + mavlink_msg_aq_telemetry_f_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: Mon, 16 Dec 2013 12:02:01 +0100 Subject: [PATCH 158/233] PX4IO upgrade improvement --- src/drivers/px4io/px4io.cpp | 66 ++++++++++++++++++------------------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 569f1e4137..db882e6232 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2562,6 +2562,39 @@ px4io_main(int argc, char *argv[]) if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); } + if (!strcmp(argv[1], "forceupdate")) { + /* + force update of the IO firmware without requiring + the user to hold the safety switch down + */ + if (argc <= 3) { + warnx("usage: px4io forceupdate MAGIC filename"); + exit(1); + } + if (g_dev == nullptr) { + warnx("px4io is not started, still attempting upgrade"); + } else { + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + } + + // upload the specified firmware + const char *fn[2]; + fn[0] = argv[3]; + fn[1] = nullptr; + PX4IO_Uploader *up = new PX4IO_Uploader; + up->upload(&fn[0]); + delete up; + exit(0); + } + /* commands below here require a started driver */ if (g_dev == nullptr) @@ -2647,39 +2680,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "forceupdate")) { - /* - force update of the IO firmware without requiring - the user to hold the safety switch down - */ - if (argc <= 3) { - warnx("usage: px4io forceupdate MAGIC filename"); - exit(1); - } - if (g_dev == nullptr) { - warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); - } - - // tear down the px4io instance - delete g_dev; - } - - // upload the specified firmware - const char *fn[2]; - fn[0] = argv[3]; - fn[1] = nullptr; - PX4IO_Uploader *up = new PX4IO_Uploader; - up->upload(&fn[0]); - delete up; - exit(0); - } - if (!strcmp(argv[1], "checkcrc")) { /* check IO CRC against CRC of a file From bccf65cc28edb8e68b38765c895e9f74604df92e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Dec 2013 22:16:51 +1100 Subject: [PATCH 159/233] mpu6000: disable interrupts during initial reset this seems to avoid a problem where the mpu6000 doesn't startup correctly if other devices are transferring at the same time. --- src/drivers/mpu6000/mpu6000.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index be4e422b04..bbc595af46 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -505,17 +505,26 @@ out: void MPU6000::reset() { + // if the mpu6000 is initialised after the l3gd20 and lsm303d + // then if we don't do an irqsave/irqrestore here the mpu6000 + // frequenctly comes up in a bad state where all transfers + // come as zero + irqstate_t state; + state = irqsave(); - // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); - // Wake up device and select GyroZ clock (better performance) + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); // Disable I2C bus (recommended on datasheet) write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); + up_udelay(1000); // SAMPLE RATE From ea107f41514ac8696ea8b94e93b9231a4802a15b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Dec 2013 16:47:28 +0100 Subject: [PATCH 160/233] Enable DMA on UART8 --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index db7a9c5c4e..110bcb3637 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -308,7 +308,7 @@ CONFIG_UART4_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set -# CONFIG_UART8_RXDMA is not set +CONFIG_UART8_RXDMA=y CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_STM32_USART_SINGLEWIRE=y From f042ea162310783a5d58d32478371f2c32a0647e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Dec 2013 10:20:22 +0100 Subject: [PATCH 161/233] Removed whitespace --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 7f73acf859..070a4e7e35 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -23,7 +23,7 @@ fi if l3gd20 start then - echo "using L3GD20(H)" + echo "using L3GD20(H)" fi if lsm303d start From 6dce57170e3ceaa3316446086f8a0cd12cc5e90c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Dec 2013 17:12:46 +0100 Subject: [PATCH 162/233] Hotfix: Fixed mapping of override channel --- src/drivers/px4io/px4io.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index db882e6232..e898b3e605 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1024,7 +1024,12 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; - ichan = 4; + param_get(param_find("RC_MAP_MODE_SW"), &ichan); + + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 4; + + ichan = 5; for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) From 9476ba522f0b174a64ff91061647bca30cf7b6ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 08:48:45 +0100 Subject: [PATCH 163/233] PPM channel count detection is now using a more paranoid approach. --- src/drivers/drv_rc_input.h | 2 +- src/drivers/stm32/drv_hrt.c | 45 ++++++++++++++++++++++++++++++------- 2 files changed, 38 insertions(+), 9 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 86e5a149a0..7b18b5b15b 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -60,7 +60,7 @@ /** * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 18 +#define RC_INPUT_MAX_CHANNELS 20 /** * Input signal type, value is a control position from zero to 100 diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 1bd251bc2a..36226c941f 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -338,7 +338,12 @@ static void hrt_call_invoke(void); # define PPM_MIN_START 2500 /* shortest valid start gap */ /* decoded PPM buffer */ -#define PPM_MAX_CHANNELS 12 +#define PPM_MIN_CHANNELS 5 +#define PPM_MAX_CHANNELS 20 + +/* Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 2 /* should be less than the input timeout */ + __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; __EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; @@ -440,7 +445,7 @@ hrt_ppm_decode(uint32_t status) if (status & SR_OVF_PPM) goto error; - /* how long since the last edge? */ + /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; ppm.last_edge = count; @@ -455,14 +460,38 @@ hrt_ppm_decode(uint32_t status) */ if (width >= PPM_MIN_START) { - /* export the last set of samples if we got something sensible */ - if (ppm.next_channel > 4) { - for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) - ppm_buffer[i] = ppm_temp_buffer[i]; + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; - ppm_decoded_channels = i; - ppm_last_valid_decode = hrt_absolute_time(); + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel > PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; + + ppm_last_valid_decode = hrt_absolute_time(); + } } /* reset for the next frame */ From 8c518aa23710ba0b9ad0c7ad2c03428ce8ddb290 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 14:25:35 +0100 Subject: [PATCH 164/233] Useful bits for high-rate logging --- ROMFS/px4fmu_common/init.d/rcS | 0 ROMFS/px4fmu_logging/init.d/rcS | 88 +++++++++++++++ ROMFS/px4fmu_test/init.d/rcS | 8 ++ makefiles/config_px4fmu-v2_logging.mk | 157 ++++++++++++++++++++++++++ src/systemcmds/tests/test_sensors.c | 87 ++++++++------ 5 files changed, 304 insertions(+), 36 deletions(-) mode change 100755 => 100644 ROMFS/px4fmu_common/init.d/rcS create mode 100644 ROMFS/px4fmu_logging/init.d/rcS mode change 100755 => 100644 ROMFS/px4fmu_test/init.d/rcS create mode 100644 makefiles/config_px4fmu-v2_logging.mk diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS old mode 100755 new mode 100644 diff --git a/ROMFS/px4fmu_logging/init.d/rcS b/ROMFS/px4fmu_logging/init.d/rcS new file mode 100644 index 0000000000..7b88567197 --- /dev/null +++ b/ROMFS/px4fmu_logging/init.d/rcS @@ -0,0 +1,88 @@ +#!nsh +# +# PX4FMU startup script for logging purposes +# + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start +else + echo "[init] no microSD card found" + # Play SOS + tone_alarm error +fi + +uorb start + +# +# Start sensor drivers here. +# + +ms5611 start +adc start + +# mag might be external +if hmc5883 start +then + echo "using HMC5883" +fi + +if mpu6000 start +then + echo "using MPU6000" +fi + +if l3gd20 start +then + echo "using L3GD20(H)" +fi + +if lsm303d start +then + set BOARD fmuv2 +else + set BOARD fmuv1 +fi + +# Start airspeed sensors +if meas_airspeed start +then + echo "using MEAS airspeed sensor" +else + if ets_airspeed start + then + echo "using ETS airspeed sensor (bus 3)" + else + if ets_airspeed start -b 1 + then + echo "Using ETS airspeed sensor (bus 1)" + fi + fi +fi + +# +# Start the sensor collection task. +# IMPORTANT: this also loads param offsets +# ALWAYS start this task before the +# preflight_check. +# +if sensors start +then + echo "SENSORS STARTED" +fi + +sdlog2 start -r 250 -e -b 16 + +if sercon +then + echo "[init] USB interface connected" + + # Try to get an USB console + nshterm /dev/ttyACM0 & +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS old mode 100755 new mode 100644 index 7f161b053a..6aa1d3d46b --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,3 +2,11 @@ # # PX4FMU startup script for test hackery. # + +if sercon +then + echo "[init] USB interface connected" + + # Try to get an USB console + nshterm /dev/ttyACM0 & +fi \ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk new file mode 100644 index 0000000000..ed90f6464c --- /dev/null +++ b/makefiles/config_px4fmu-v2_logging.mk @@ -0,0 +1,157 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_logging +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm +MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += modules/sensors + +# Needs to be burned to the ground and re-written; for now, +# just don't build it. +#MODULES += drivers/mkblctrl + +# +# System commands +# +MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 +MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control +MODULES += modules/multirotor_att_control +MODULES += modules/multirotor_pos_control + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Unit tests +# +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index f6415b72f2..096106ff33 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -78,7 +78,8 @@ static int accel(int argc, char *argv[]); static int gyro(int argc, char *argv[]); static int mag(int argc, char *argv[]); static int baro(int argc, char *argv[]); -static int mpu6k(int argc, char *argv[]); +static int accel1(int argc, char *argv[]); +static int gyro1(int argc, char *argv[]); /**************************************************************************** * Private Data @@ -93,7 +94,8 @@ struct { {"gyro", "/dev/gyro", gyro}, {"mag", "/dev/mag", mag}, {"baro", "/dev/baro", baro}, - {"mpu6k", "/dev/mpu6k", mpu6k}, + {"accel1", "/dev/accel1", accel1}, + {"gyro1", "/dev/gyro1", gyro1}, {NULL, NULL, NULL} }; @@ -137,7 +139,7 @@ accel(int argc, char *argv[]) } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { - warnx("MPU6K acceleration values out of range!"); + warnx("ACCEL1 acceleration values out of range!"); return ERROR; } @@ -149,20 +151,19 @@ accel(int argc, char *argv[]) } static int -mpu6k(int argc, char *argv[]) +accel1(int argc, char *argv[]) { - printf("\tMPU6K: test start\n"); + printf("\tACCEL1: test start\n"); fflush(stdout); int fd; struct accel_report buf; - struct gyro_report gyro_buf; int ret; - fd = open("/dev/accel_mpu6k", O_RDONLY); + fd = open("/dev/accel1", O_RDONLY); if (fd < 0) { - printf("\tMPU6K: open fail, run first.\n"); + printf("\tACCEL1: open fail, run or first.\n"); return ERROR; } @@ -173,45 +174,21 @@ mpu6k(int argc, char *argv[]) ret = read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { - printf("\tMPU6K: read1 fail (%d)\n", ret); + printf("\tACCEL1: read1 fail (%d)\n", ret); return ERROR; } else { - printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); + printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { - warnx("MPU6K acceleration values out of range!"); + warnx("ACCEL1 acceleration values out of range!"); return ERROR; } /* Let user know everything is ok */ - printf("\tOK: MPU6K ACCEL passed all tests successfully\n"); + printf("\tOK: ACCEL1 passed all tests successfully\n"); - close(fd); - fd = open("/dev/gyro_mpu6k", O_RDONLY); - - if (fd < 0) { - printf("\tMPU6K GYRO: open fail, run or first.\n"); - return ERROR; - } - - /* wait at least 5 ms, sensor should have data after that */ - usleep(5000); - - /* read data - expect samples */ - ret = read(fd, &gyro_buf, sizeof(gyro_buf)); - - if (ret != sizeof(gyro_buf)) { - printf("\tMPU6K GYRO: read fail (%d)\n", ret); - return ERROR; - - } else { - printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z); - } - - /* Let user know everything is ok */ - printf("\tOK: MPU6K GYRO passed all tests successfully\n"); close(fd); return OK; @@ -255,6 +232,44 @@ gyro(int argc, char *argv[]) return OK; } +static int +gyro1(int argc, char *argv[]) +{ + printf("\tGYRO1: test start\n"); + fflush(stdout); + + int fd; + struct gyro_report buf; + int ret; + + fd = open("/dev/gyro1", O_RDONLY); + + if (fd < 0) { + printf("\tGYRO1: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tGYRO1: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); + } + + /* Let user know everything is ok */ + printf("\tOK: GYRO1 passed all tests successfully\n"); + close(fd); + + return OK; +} + static int mag(int argc, char *argv[]) { From 3ad9dd030c01e233a78aebfd2e20e67168962255 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 21:10:33 +0100 Subject: [PATCH 165/233] Added performance counter for write IOCTL --- src/drivers/px4io/px4io.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e898b3e605..9e84bf9291 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -244,7 +244,8 @@ private: int _mavlink_fd; /// 0) { + + perf_begin(_perf_write); int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + perf_end(_perf_write); if (ret != OK) return ret; From f174ca3ce5dfe338b79f52de46f127abf1c3aca1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 21:52:10 +0100 Subject: [PATCH 166/233] Added average as direct output --- src/modules/systemlib/perf_counter.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index bf84b79450..b4ca0ed3ec 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, + pce->time_total / pce->event_count, pce->time_least, pce->time_most); break; From 0f0dc5ba068d24fb8b339acc8ef850f5f6ea9e47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 12:45:04 +0100 Subject: [PATCH 167/233] Allowed custom battery scaling on IO --- src/drivers/px4io/px4io.cpp | 17 +++++++++++++++-- src/modules/sensors/sensor_params.c | 1 + 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9e84bf9291..df96fa0bbd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -896,8 +896,21 @@ PX4IO::task_main() /* re-upload RC input config as it may have changed */ io_set_rc_config(); - } - } + + /* re-set the battery scaling */ + int32_t voltage_scaling_val; + param_t voltage_scaling_param; + + /* see if bind parameter has been set, and reset it to -1 */ + param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val); + + /* send channel config to IO */ + uint16_t scaling = voltage_scaling_val; + int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); + + if (pret != OK) { + log("voltage scaling upload failed"); + } perf_end(_perf_update); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2aa15420af..78d4b410a8 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -195,6 +195,7 @@ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else From 3e037d40de2a68b99aa4600f060eab3555f75832 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 12:46:06 +0100 Subject: [PATCH 168/233] Fixed bracketing error --- src/drivers/px4io/px4io.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df96fa0bbd..a7f7fce453 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -901,16 +901,19 @@ PX4IO::task_main() int32_t voltage_scaling_val; param_t voltage_scaling_param; - /* see if bind parameter has been set, and reset it to -1 */ + /* set battery voltage scaling */ param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val); - /* send channel config to IO */ + /* send scaling voltage to IO */ uint16_t scaling = voltage_scaling_val; int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { log("voltage scaling upload failed"); } + } + + } perf_end(_perf_update); } From b2e527ffa6f24a67903048ea157ee572f59f98a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 16:13:04 +0100 Subject: [PATCH 169/233] Counting channel count changes --- src/drivers/px4io/px4io.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a7f7fce453..b80844c5b4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -237,6 +237,7 @@ private: unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values + unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels volatile int _task; /// 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); From 831f153b7385fecb180c977727eb6b2f8bef1317 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 16:37:45 +0100 Subject: [PATCH 170/233] Add tight RC test --- src/systemcmds/tests/module.mk | 3 +- src/systemcmds/tests/test_rc.c | 146 ++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 150 insertions(+), 1 deletion(-) create mode 100644 src/systemcmds/tests/test_rc.c diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 5d5fe50d33..68a080c77c 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -27,4 +27,5 @@ SRCS = test_adc.c \ test_file.c \ tests_main.c \ test_param.c \ - test_ppm_loopback.c + test_ppm_loopback.c \ + test_rc.c diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c new file mode 100644 index 0000000000..72619fc8ba --- /dev/null +++ b/src/systemcmds/tests/test_rc.c @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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. + * + ****************************************************************************/ + +/** + * @file test_ppm_loopback.c + * Tests the PWM outputs and PPM input + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "tests.h" + +#include +#include + +int test_rc(int argc, char *argv[]) +{ + + int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + struct rc_input_values rc_last; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + usleep(100000); + + /* open PPM input and expect values close to the output values */ + + bool rc_updated; + orb_check(_rc_sub, &rc_updated); + + warnx("Reading PPM values - press any key to abort"); + warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes."); + + if (rc_updated) { + + /* copy initial set */ + for (unsigned i = 0; i < rc_input.channel_count; i++) { + rc_last.values[i] = rc_input.values[i]; + } + + rc_last.channel_count = rc_input.channel_count; + + /* poll descriptor */ + struct pollfd fds[2]; + fds[0].fd = _rc_sub; + fds[0].events = POLLIN; + fds[1].fd = 0; + fds[1].events = POLLIN; + + while (true) { + + int ret = poll(fds, 2, 200); + + if (ret > 0) { + + if (fds[0].revents & POLLIN) { + + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* go and check values */ + for (unsigned i = 0; i < rc_input.channel_count; i++) { + if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) { + warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); + (void)close(_rc_sub); + return ERROR; + } + + rc_last.values[i] = rc_input.values[i]; + } + + if (rc_last.channel_count != rc_input.channel_count) { + warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count); + (void)close(_rc_sub); + return ERROR; + } + + if (hrt_absolute_time() - rc_input.timestamp > 100000) { + warnx("TIMEOUT, less than 10 Hz updates"); + (void)close(_rc_sub); + return ERROR; + } + + } else { + /* key pressed, bye bye */ + return 0; + } + + } + } + + } else { + warnx("failed reading RC input data"); + return ERROR; + } + + warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!"); + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 5cbc5ad88f..a57d04be37 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_rc(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index cd998dd18d..1088a44076 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -105,6 +105,7 @@ const struct { {"bson", test_bson, 0}, {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From a707e8cdb32b4b1b9c66b3e4010b56a2b0188a99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 18:58:28 +0100 Subject: [PATCH 171/233] Added missing folder --- ROMFS/px4fmu_logging/logging/conv.zip | Bin 0 -> 10087 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 ROMFS/px4fmu_logging/logging/conv.zip diff --git a/ROMFS/px4fmu_logging/logging/conv.zip b/ROMFS/px4fmu_logging/logging/conv.zip new file mode 100644 index 0000000000000000000000000000000000000000..7cb837e5666f51eca7ed803dfef4581f01bec1f3 GIT binary patch literal 10087 zcmaL7V{j!v*Zq6qi8b-WwoYu@b~3Ruv27bCwrx&qn-kl{1b3eIR^58)|K5A6t7~_E z=vCEK>&x2vrzisf`5gcNd;_41Ac~;1hZwjZ007J^000u;2f)e1#@?L8z{JJYfx*E; zO%)aZK`{rbEVu%z?BWg&0E2u40|5T#eYduc{RTVwSLf&Vs=&Ob3ipT&^VL98!Sy1Y z`jRQoLtoBNXx?aYwOR?Vo0Bcn=lM9cNHVfu(FC6YGInbJ!=0DlI%Mz9e(ia~%Nx6q zfl3ScYmK(K4KOmwMkJ7^rm`bXNH9?@2Lod)Dp+lHynv?9F6TIVDKiV;rFkIKYz;3Bf5Ch-qX|N?qkU9?pV<-K9z`#ZbF6`CW_Fi+w2FcLJukytO^TKNyog*Lu3Qj0$T}|*x*y#1#v+PKorgM^R zci~g&oh8_tM7z^CS6E7wz_-$h)(;mciFG37LA`wkS-0%~oJ8JYPK(q+;$$+}g^O5* zQbUwNC6r(`ItwsY5&3YpaT?7{tX*5Z8Ucz)2=0?cNd6PRL*XexdQnJf+{VsQX0v{FM`>qGo((^C9 zoQa$tpBp0biCrt#z`waxLtQ%8>Cw9jl=iIRlszR~JIPo`;~ei~`+Uj!kU$U-O)J>5papx6c~e1D-*AboN=+cARG*U2`IK;r z`dBCl>^h2Giz5tO-#;q$%LM2uImOFTF z15Tl2KU2flP8UMuZ0IQMfdE;xIr1Vx0MiW7vI=BEJUgqlMso3ED2~d;fTGyTPYxg; z9-6ci8j)y=qRNm|_k;7W;z9R>sv+M;G|o!!ry4r$uT&H-P`xgZOV7b+xMlJ3UHAt| z@7~zlQ}`x{vTAg);NzH{a`jkJEw8Y^Aqow%jTzZ6eJX`%$d+i6O(s1yy~SbG7)l%1 zU(w688v4u)Egv_do{)S*&dm!u;3+hy7QZUW@i9_CtH83k%?;#8{3Zc&kDCRzBRx1g z8yx@Iln4M4SsiA=;>rui0I&E5fR+6#r$DL_^wrW~&v_!&RXQ>ur zZ*mMrdrSxh;upU$c#_rI5yyqBBu`RsFqv_g8ZWI-DqQ98XN2c)Lp5Zyq1#n*GB$jB zmnCIsWFz9e7G?WsWp=9L6kZ)^m%WNtwFKj4zfVAXsAKLHu*%Dy!S-}J@U*lcw$6+0mql^qO1PMlw9WCzcc5TmM zFB76W{8^t4>+G*F8kc||q0jo!2o_r^p+}&&p&9+Knt1+gLJrI_$tJMo;Kh;$c|?RB69 zO@YF&0_P~_y{kw{t#mtt+WFgEw^QxQpj_{+?oPHH=-8Z6V0iG~kRkI>x3a zL{Mx}cWib{tDX{_YD`_MAgqCDbflvmIW#frWum#j6n0?2MR4AQ_+ZNU$vAfE5S590 zV7-e{p)gTEWpB9}*6+N>G7du#Gqvc>nj3kwn1VtpOdRuIqr0yVf#rTP*8ZkytGX0Z z!=}Art+{j;bJfYDpo4pfYudZd&p6Y1z6v>f=Je*3TK7w1G=j=O9>IdXn5Tlz!GCOY z6{>`>ekR}{AXS^b8)^Lw;iAPx$5xKPXT7B35;++&v7#;PvA@0IXIC>qOMOuF;-ElF z`q#s{mduj0M~7Xd57Rb*bk2KtE7$_)(CFcp4W-#$(Z;nLolzwp=}0x54^g`gg4~DE zo$Am_ZlV;tsmc5%OAK&qGL>+fQpe6z|HoyS@gI?i1;fz=_$(%g40`W7GP*)C50(sJ z>%_S3*ZgA!_ye50zRwHdi!%rHdVy=B+*A*cs_{%~{3hmA&dPS?^mgV@cknhTBWp{s zm?@IJsV%zgv=(YyH8%y(+gU3Fg{?|y%AwM&P%Umg>r$bsnkEL18!kSF@0_om3wnE! zuZXmy-hR)zAZ(Q|1$I;6=pF|AYsS^f8evZTvnp3CTxpe)w@3t|Xozy5i>6@y0D9&T zkkB{gISID5nRHt+Sd|=zqZx9|%)pg=zAOy@fJlGa6`UJ-$|Do z>BxN%eHD8r#_!`l5x=y5&u^jsRD=%~0Qoc;IFg>w-^;?I)cz6pt$IS*6iVrHUfjIK zpLZ6=>RO#oBdy00(Q(&?5!*9cH(Bu&Wvz7CqQL#p)27bGjnJd)h(UpL52WN_&o~IC z$Zj}s>eD=;d_l9?lvkFWdd}%>k99_D&_5qK;3s~_nMWv+LQb@pk5GhtJ0LNk3Vn4!u|;lO6lX*&?%l6pECcO_?=dSHVj38fimtMTIZ) z9$?i!LT6UI?Iv4+TA?zyZ`oBd5Grj>?ut$diMKdSiH)xYN?t7qkK0{)p4cCT!IQwa zDYZ)Zt9eI&>Xl9S_N%ewPskVEnRnh^6wP zngawH0)ad-_pK|_B7q5TPskoUP;2zi3TesJFl5LoYGjg?ub`>H@A!@ch1T)oZI4&b z{^jXm&zspibReOBvNGtht7Cg>n8262R@V*R%ad`)!pZ0M*Hzam=kt5}3QRvP%HqH~ zyq)mV&U1we66s7OV>cSLBJ{OLM-g}qEmHG7{6>!uB>85==vmiePnZF)pPooL!(>P> zmdXS}Qf<~05u?8$l$%*tgB-J4f&7--_i#o1W@nTsra>FLvd?6*Hxh>e=*R;MgSt95 zf=C4(ZIFoJSt_Ag>r3<_w=KmbpGAI4IaG&Fj5XLm8N(Dw6$&3g! zQBwnSZU+s`_$D3qg1S}_uOS=iV|+g~r*J6}OX9>QQ7Sz3^>gOVM~DBC$d z9^G%3Io;Wp+`a|I?QcO6#5Vuo)2#o)g39c{^B$CE!BG|+R4sy6^5_#d`@Q*R}%M;35W8W^=(wGmS9CpEpjc`pauEG-<_5ah)ED?Bp!olU7x91Lh0{ik zpBf7QFgF7LVE=`ef6>L*-p-Z5_J49q@c(;mx#Y99-;hk)hDraLZG;I?79R6ll_v@7 zvohhDtD(n9%#u~N#s-Q>Z7LSUA`3WreVsb=e*UgJgIOz*jF3P0fy8MUhi!t$9;R?$b$?bb*I>mPS|;jD%ymor?2LkH+B$W$ zcpJ<%-l+YZJbFkNW{#yX=xDeoi|pK1E3vcOf(OYY?RJxEC*aF!-fZ9(@QeJ;QGqaa z@bzY3>hRTp1l1-8=NM4S#MJpJcZwslsv}SR^>U|!`w5j zZ{88}_vg=5Xy0Ait(M#QM>*GagGFhR%}m+1Amsgz{^EE!3KVNT&4Os&j(mxCiLYrO z_)6s#$#41%GMNR4vEB)pi*i&blTv+qL*zp;SZh#c^u804*)%WLxP*@Tc<>c z3yT|H@8&(k&-J5mXK+VB!z!#D-8YdVU9=j7oQY+0m5k{NkmKPZf-G_w)7Zy6OR8=* zHldXXV3KKbX+2c`iPVZJ2ypgj0iCDTT55BKoMudaBb-!^n+_d#QrI@_qDw`%3~m31 zZn?(}MteUvxukN5Z5pVjCsWO?`E5n3j^O4wrgVF|N^e_u_cts)tznSa5eY8lt+4e& z#RwCMp?2tTtrT7eG%ILmwr zeq?F~j8|;4JHeGYbtSA!q)26k*qUyJQV!A|0W)Fl;bSjwcG+k!zB9pXVO4sX`UaW$ z5>BQH7CN51#9Q90qZX8~E2`lR<$`5~HN8%@Eo!^d2-5I|9T)7*1$!jOMfz|T3u?iN zM;98@wirL5dMszG5x!kLfcN)6T{$nl%6jH{7BVKq)dgH*y;EDPOa6|d!~EPbwOZTl zfJvoff-?O}ju?>eXD1n9W>lNJ`Nm3iT6_Th zuAbv8VOP+99bJD$$l`0ykrTw^_Rz$78s7lniVhTfiB91|2kYhD`N{LGQ!d>c`72-3 ziSUn;&~LqP=GLQug^C^_Vgh%f6y0!wu6`1 z)G-Cym3Ipwr?=-9*XHM!`LnC5M8J5-O5oJw`c)hk!k;n@oX5V{gj=lHGx)pmm23ow zzd7`vtDQXK4HMHdeofnlFdt`mXhZp|H=>#MKM>?@F&I=s4wy`>=g+ z42`nS4p=M_lRn?B%y8?Mtqe;Y+8@=CzKbv*of5u*jg!R)%!gxpi*QK97m2BxYf)0& zKb*wz;I?}Ne_M_Qe+4zX8oml~1?Hnc`6Ww-Jod*S2leG5OsZwPSR=EKd1z^gpumyr z%=q^lP0{IATc1nZl_q{sDrets`+rQX#svvPN>i&x0F7lqeasD}=`O+2mZ5k$kN!V|1c^&YLFmZk9*~uZ~`^pd z4@rMp=IHIuQsKaNtu#-!c)IyUj7DAEi>)<>kk(jbA-U_c^UKKHP&Gr+iiRiDERd-aq zh+P|5&?=vd1lEO5l}SL%lD@Ye#7PX^oYQtw>jBc-Z=_4(sXCe%|a5Z%X@_c`t~jZpY!8Khe=AhRrA} ztx`6GI;tYfd6Ax|U5%-O4AliIh7rMSS_~nWQ>}ubO_!*RxYc6svR_&7iPOS*W)rT{ z{sTocP^EJj(@gPCp1K-?;j%PaEh@@)!S@RWS(M>C-QGa)YG41P1XAB$`1cv0Lp|ML zd8wKflNxdLocgrDNtC1?R@gGMwVW( zv#~D9Jn7-T0$WdE1#5?-NtV=~N*Q^+xmWZQNqPky8P4;T^IgQw66Gt+JN8S?HP@lR z!u}C*GlG>z)s;bj=%uSps-m2MY8 z8mS}lGXRe!RTk&Sqq-(Ci@8+K@|j%V*I9Gx$iq44a=E zZRXBV%M80qLPqTLfy=5ZQV}Gfdm#%dE8|~I#D^GWrsN{OQ$nG^m zZEE`6?L*6KK{A+aiIt{keG)%dAC#ZmfoiNC*~vMOb>QlZa?@hOLoTZaCI~e2X?Q|q z8$a|@mh`}Asz+6%;na*;Wur}OO}=q%^wOZFY0z5C4#~S25o22u6r|HY*9*TLf9^QH zWB}I>1`gA8kw$#zu8Rj=&3;UWX+!eNSn8sbLMZ`SYL zn=Pl`fs}ErPIr&4@;z(QW38QB9-F?IV`BrCS-dt$_q#AGInEUOK(APurk7-s+4fGN zHr4><)@Vcq;2CoXVg_Qv`^v^FJ^V`Hc;hv^`vKVJU#=7#-n-r(2)r($PRC7jQEmlj z$R{q?gCXC1LXXw^)Kk+ls#xyf%%`&rFw{xuoHE^CSVM=tTUd4C!(g8lP_$ zFJG2gNt(vm76ED}n);19GcJq}s5LC@kB44z&p_k3#$47Ju_|NKVrqfCL#79(Jac)003n)Ho5FmCLLucf?$~yaQULXV?J$|;#>DWYy{&u;Rb!6rrStyGTf}a7Q z^5Rk1NzOB|+}6?OAVt|z#Ad*TiPImk|F51y=(vY|R zx4ksxT6MpP8$#i?wb|6e^>RJM=;nTftxj2fcz4tdXJ2kTErLqm3H>V|ai z4}S}`&C5B;yKTD4SCF)uhmH16Y)e`o<|Bz*nn5y?IO07&+{>X16>u4J_Up_bw?(+GRfjY;Xj`uoKS-D<5` z-L_GmuLRUze5{M-j=IsV1hJ#LdnjrV6t(I$qdFvrRgbTf&da^gs}bmuG6D8ew+ajmg4rk>39hFvuwM4 zt|j}Ornf`Sha${!v_32_{G@XjJ^xmi*x^dv_H^9G*LG-4sZRz`E3(*MJNED|o*nX0zN$Qs-T%wj8mXE&byq zMD8g+;Np{?)ouGtqQWP^JD*~KsG<&7Uv}WMb>vxaWeK6XPJ=T}IUV+=f&06!wXjS; z4i9DT=9-&=zXHp3y1%$uc`yzjY)9n#GvoruL$-4k>(jX}3AMKs#&WvqU8-2Q%%-yJ z;1^yIqe8%gEnd-YCCPD$v1;B#OX+^BjPPn;g&hG2U@G>Be#B8N+4}C#$GmIAx8?JEk|jlYP2*=hR+NO+=l=0vr=~!=)MWas;{X{(2!D>!x-DX zh9-EiLLq7~jzi|l^s|V0GJ-BEf7f7lBvBu3)@VefffTE|hUsHc{~$s^4WZmkCb_nS zAB-0BXf##|C3y(6xY#l9U0&}R^XE+Y8qX7j%V>PPC8LVz7PP#AlD1}4M0y54v|v<> z{Z}6CBBuP)i0kbes+#taHsP4ZbRm~_s*$X31U75CX*VjcjJO3O2eWn=3&y%H*5W)zUjYIcQx^7 z1YYh-y(2JsRXgW{21biscnn9&A|0WTBLq#?WiIp{OKbuXXp>ZS|?(zog175NCKj6hNK{q;Q5dP9?Fpevn3hD+iR`2!Klnfuz*MP{iSAXQ5-4S7^yCK^u?fO` zZzF$Aq9YA8TS}wNNcAh-SdPvnp`RGhJ9L9n!Pkwq?rDLGcfOrMa z`|{D(#~T|sHb8FhUH0SV!KqU;u0$O7?9qp9$6MFVG+gT54blp1MM)eQeGVFV&~Uz2 z>rS1?PU~M`WOS)o)bJ{0L$GViE0f{M|C}0{DYUKj{>(E-)+%NT5{=-26*ZQqd-X*5 z^pabY<$M{frMfE2q0na)f+6##B)~C@|C}jxW_$~ZnkIbVho8U9%KiFt`uTXAJvrk( zT-^2J&!-66wf|Jz$b07BM?F6WHhw=s+a!*?;yA9jP4D?bCtPQMpOf+QfcY&z32ye7 z;=*Ql9@z6{>w4dE?6IyJ)-A)i6vG~I_gC|&i*1Z2k;>SgBAGmRI|>}~^;kekOZXzY zEKYTPK8;b(KX!sZuMiu%BdAo>{+7HtD&%yxQv*L>%KiY|OXHlchZg)V&TL1oo3qKc z-C<%hX>Pw5GK|MTu6SiuPk)A^Ajt~Ks=!G&BgUjqPexIv+20ZHZ&z}w>~p_&7O)EO z1caB#HdXkPuX$ygOFYt;Fp_<$PZHL{eXgC_M-C51+D6VDnV-v&l)`;%PS96J*c62M z<9srzb=K0{yA?BBO>t>%O?kjyD`|5P@eRpwwE$J1fZt)6mH9#FUX+$85(?S#NwRlH z@y6MUs$z@VW*LJJNr@ppg(!Dterc@r(kUhs_6rriCudD#%^QO{JEUjauTH^y8eICb z6;AH88J_a&i;e83Zz1r#3lBDexLCCgWAe-;PH3y&!I2D*pu8E!?r=y^S8qp8*inR; zTzTc`19pHpT1Y3HkUGyV>a#rK7>b$-ziV$1LT~U+y;Rp^;@j~)np$i3!Vh>T#)eBF zW185bV8h%cr>&{hP~JKeCNeq?pp4`*2wz42K1X(F6n>GXQGM8jUVbVz{KXhvk{ir+@oGBgyzl+>}9XlZEtoo3hR6PsES$x z2jT_EH3IBNa6{OY$9A)v?=KPX)>WCCa$7gScMIL5ooD-ldDd$>j)yg0e;%;G6tV0| zcMcg(HR?fll5ETzda7@uI{j&swj^Ax$|eMZ>8i@ECJh_{jVdgvl|w-qv1moI7>JhF zX*Ku)ZOserygHlC%Y<1CtFR1jTYp}TGs4T+yZ}Rrw-SDB6u4u|BaV50EEe9i{?H;U ziHj|Z`?^-#@!wg)W~w&Ik5-$X87L^nuDZ>T=>om$`3ogC*nH7L5Ubj2DT(X`wA}-2 zn2K2(g&`f4GCyykS(XU>&a$2r1YDs@kGpdz6?pZ6DDM&@@ZJX0;Sqpd0ms=_TyHd1 zRb91(^(}JdsB{ylxj&?HH)V=L6Z!H)=6Uz0#WMe_Sf8@5Pbt?@RQ4H_&4#TvOu3RM)JP-W(sg zDW4d=z4ipO+`p zY00=CwTB`Ti@zpM^yNz_AK5{LwLR^|qgDL|KeD}D$J1r#9J0rkIJsD_Q6}`JVzrVpx?F1eg)3m zA&*Bp69#oz7;{xpIdZ}Kyk~N#WiM`-SC34mxkcqWvBArxV3%!-5Y>Kgie# zdN920HKh$PHt4-z!5ObncNxV_o$?)|?AQ?5HJ{3DYba@|K(SA!?MU}Y$@xlUXI2iF zec=!9S^B*7UG~eL9fd+YCGR?brr$){5FbzW1Tu5~@-d`vYbZrUTQ%Ugd z$BF-tn8O~yCZe4kSDgNmN73>$V8n_Fpl5z!5m|phF_BSLBE;i5D996k)xXYTHNh^T z`~oP-fPoW2{D1Lf*#EdQ0N|gC07U`+IsKos8PxwxoBbc8*?(*P@3GH+Yj*!5+5T$? x^iS?TL!kd3x&Q6#|CXaB`{(!n^7%i#|M>iW=|MsN=Q9xh*8RV#Pw}7I{{>mxP2T_j literal 0 HcmV?d00001 From 999051546a9dec7cc4eeef8ddc7c37f6c8e68b00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 19:08:52 +0100 Subject: [PATCH 172/233] Fixed compile error --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 194d8aab98..75e6d4dea5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -136,8 +136,8 @@ controls_tick() { perf_end(c_gather_ppm); /* limit number of channels to allowable data size */ - if (r_raw_rc_count > PX4IO_INPUT_CHANNELS) - r_raw_rc_count = PX4IO_INPUT_CHANNELS; + if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) + r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; /* * In some cases we may have received a frame, but input has still From 6c990d0a6e6d0888fc8c2cbf9eba1b84637c8d4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Dec 2013 20:44:51 +0100 Subject: [PATCH 173/233] Fix usage of wrong constant for RC input channels --- src/modules/px4iofirmware/controls.c | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 75e6d4dea5..f630b6f2a7 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -66,7 +66,7 @@ controls_init(void) sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -113,7 +113,7 @@ controls_tick() { perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } @@ -210,14 +210,16 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < PX4IO_CONTROL_CHANNELS); + if (mapped < PX4IO_CONTROL_CHANNELS) { - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + + } } } @@ -334,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > PX4IO_CONTROL_CHANNELS) - *num_values = PX4IO_CONTROL_CHANNELS; + if (*num_values > PX4IO_RC_INPUT_CHANNELS) + *num_values = PX4IO_RC_INPUT_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; From 9abf31c2baaa59b2b7727b87470d3575f3a099b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Dec 2013 21:09:47 +0100 Subject: [PATCH 174/233] Support 18 channels correctly on FMU --- src/drivers/drv_rc_input.h | 2 +- src/modules/sensors/sensor_params.c | 18 ++++++++++++++++++ src/modules/sensors/sensors.cpp | 4 ++-- src/modules/systemlib/rc_check.c | 4 ++-- src/modules/uORB/topics/rc_channels.h | 11 ++++------- 5 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 86e5a149a0..b5fa6c47a7 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 78d4b410a8..c54128cb52 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -190,6 +190,24 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC16_MIN, 1000); +PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC16_MAX, 2000); +PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC17_MIN, 1000); +PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC17_MAX, 2000); +PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC18_MIN, 1000); +PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC18_MAX, 2000); +PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f99312f8c6..d9f037c271 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -165,7 +165,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ @@ -602,7 +602,7 @@ Sensors::parameters_update() float tmpRevFactor = 0.0f; /* rc values */ - for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { + for (unsigned int i = 0; i < _rc_max_chan_count; i++) { param_get(_parameter_handles.min[i], &(_parameters.min[i])); param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index b4350cc243..21e15ec563 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -45,7 +45,7 @@ #include #include #include -#include +#include int rc_calibration_check(int mavlink_fd) { @@ -66,7 +66,7 @@ int rc_calibration_check(int mavlink_fd) { int channel_fail_count = 0; - for (int i = 0; i < RC_CHANNELS_MAX; i++) { + for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { /* should the channel be enabled? */ uint8_t count = 0; diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 5a85801439..086b2ef150 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Nils Wenzler - * @author Ivan Ovinnikov - * @author Lorenz Meier + * Copyright (c) 2012, 2013 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 @@ -47,13 +44,13 @@ /** * The number of RC channel inputs supported. - * Current (Q1/2013) radios support up to 18 channels, + * Current (Q4/2013) radios support up to 18 channels, * leaving at a sane value of 15. * This number can be greater then number of RC channels, * because single RC channel can be mapped to multiple * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 15 /** * This defines the mapping of the RC functions. @@ -91,7 +88,7 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAX]; + } chan[RC_CHANNELS_MAPPED_MAX]; uint8_t chan_count; /**< number of valid channels */ /*String array to store the names of the functions*/ From f8134c9c67863aec8bc0cf9a12d602cf6dbc5bc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Dec 2013 21:12:31 +0100 Subject: [PATCH 175/233] Enable 18 channels on IO --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 2145f23b9c..dea04a663b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -54,7 +54,7 @@ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_CONTROL_GROUPS 2 -#define PX4IO_RC_INPUT_CHANNELS 8 // XXX this should be 18 channels +#define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ /* From 64ad3d7e0a60e994f6f26c18d52f4b2fd8b8beb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Dec 2013 18:44:07 +0100 Subject: [PATCH 176/233] Added channel count to log format --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2adb13f5c2..e94b1e13cb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.chan_count; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 90093a407c..ab4dc9b00d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,6 +159,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t channel_count; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -281,7 +282,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), + LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), From 107bb54b33dd4360fd5fee538f7a87b79279b8ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Dec 2013 20:38:09 +0100 Subject: [PATCH 177/233] Robustifiying PPM parsing --- src/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 36226c941f..f105251f0a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -342,7 +342,7 @@ static void hrt_call_invoke(void); #define PPM_MAX_CHANNELS 20 /* Number of same-sized frames required to 'lock' */ -#define PPM_CHANNEL_LOCK 2 /* should be less than the input timeout */ +#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; __EXPORT unsigned ppm_decoded_channels = 0; From c19159762520a43520981e7fab2c3f5645bc279c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Dec 2013 15:04:11 +0100 Subject: [PATCH 178/233] HIL: only listen to first 8 actuator outputs --- src/modules/mavlink/orb_listener.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index c37c35a63f..9e43467cc3 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -499,8 +499,8 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[6], act_outputs.output[7]); - /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + /* only send in HIL mode and only send first group for HIL */ + if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; From a5023329920d5ce45c5bf48ae61d621947cdb349 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:10:24 +0100 Subject: [PATCH 179/233] Greatly robustified PPM parsing, needs cross-checking with receiver models --- src/drivers/stm32/drv_hrt.c | 39 ++++++++++++++++++++++-------- src/modules/systemlib/ppm_decode.h | 1 + 2 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index f105251f0a..5bfbe04b8a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -282,6 +282,10 @@ static void hrt_call_invoke(void); * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). */ # ifdef CONFIG_ARCH_CORTEXM3 +# undef GTIM_CCER_CC1NP +# undef GTIM_CCER_CC2NP +# undef GTIM_CCER_CC3NP +# undef GTIM_CCER_CC4NP # define GTIM_CCER_CC1NP 0 # define GTIM_CCER_CC2NP 0 # define GTIM_CCER_CC3NP 0 @@ -332,10 +336,10 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */ +# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */ # define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ # define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2500 /* shortest valid start gap */ +# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 @@ -345,6 +349,7 @@ static void hrt_call_invoke(void); #define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT uint16_t ppm_frame_length = 0; __EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; @@ -362,7 +367,8 @@ static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; struct { uint16_t last_edge; /* last capture time */ uint16_t last_mark; /* last significant edge */ - unsigned next_channel; + uint16_t frame_start; /* the frame width */ + unsigned next_channel; /* next channel index */ enum { UNSYNCH = 0, ARM, @@ -447,7 +453,6 @@ hrt_ppm_decode(uint32_t status) /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; - ppm.last_edge = count; ppm_edge_history[ppm_edge_next++] = width; @@ -491,6 +496,7 @@ hrt_ppm_decode(uint32_t status) ppm_buffer[i] = ppm_temp_buffer[i]; ppm_last_valid_decode = hrt_absolute_time(); + } } @@ -500,13 +506,14 @@ hrt_ppm_decode(uint32_t status) /* next edge is the reference for the first channel */ ppm.phase = ARM; + ppm.last_edge = count; return; } switch (ppm.phase) { case UNSYNCH: /* we are waiting for a start pulse - nothing useful to do here */ - return; + break; case ARM: @@ -515,14 +522,23 @@ hrt_ppm_decode(uint32_t status) goto error; /* pulse was too long */ /* record the mark timing, expect an inactive edge */ - ppm.last_mark = count; - ppm.phase = INACTIVE; - return; + ppm.last_mark = ppm.last_edge; + + /* frame length is everything including the start gap */ + ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start); + ppm.frame_start = ppm.last_edge; + ppm.phase = ACTIVE; + break; case INACTIVE: + + /* we expect a short pulse */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; - return; + break; case ACTIVE: /* determine the interval from the last mark */ @@ -543,10 +559,13 @@ hrt_ppm_decode(uint32_t status) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + break; } + ppm.last_edge = count; + return; + /* the state machine is corrupted; reset it */ error: diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h index 6c5e15345e..5a1ad84da9 100644 --- a/src/modules/systemlib/ppm_decode.h +++ b/src/modules/systemlib/ppm_decode.h @@ -57,6 +57,7 @@ __BEGIN_DECLS * PPM decoder state */ __EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */ +__EXPORT extern uint16_t ppm_frame_length; /**< length of the decoded PPM frame (includes gap) */ __EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ __EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ From edffade8cec2ea779040e97fb5478e0e9db12031 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:11:48 +0100 Subject: [PATCH 180/233] Added PPM frame length feedback in IO comms and status command - allows to warn users about badly formatted PPM frames --- src/drivers/px4io/px4io.cpp | 10 ++++++++++ src/modules/px4iofirmware/controls.c | 10 +++++++--- src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 6 ++++-- 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b80844c5b4..010272c6f4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1724,6 +1724,16 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); printf("\n"); + + if (raw_inputs > 0) { + int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + printf("RC data (PPM frame len) %u us\n", frame_len); + + if ((frame_len - raw_inputs * 2000 - 3000) < 0) { + printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n"); + } + } + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 194d8aab98..58af77997a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -50,7 +50,7 @@ #define RC_CHANNEL_HIGH_THRESH 5000 #define RC_CHANNEL_LOW_THRESH -5000 -static bool ppm_input(uint16_t *values, uint16_t *num_values); +static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -125,7 +125,7 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); if (ppm_updated) { /* XXX sample RSSI properly here */ @@ -319,7 +319,7 @@ controls_tick() { } static bool -ppm_input(uint16_t *values, uint16_t *num_values) +ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; @@ -343,6 +343,10 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* clear validity */ ppm_last_valid_decode = 0; + /* store PPM frame length */ + if (num_values) + *frame_len = ppm_frame_length; + /* good if we got any channels */ result = (*num_values > 0); } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 11e3803976..500e0ed4b3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -124,7 +124,8 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */ +#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3f9e111baa..6aa3a5cd26 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -89,7 +89,9 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_IBATT] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0 + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_NRSSI] = 0, + [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -114,7 +116,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon }; /** From 8d2950561d1889ab1d4c2fc5d832a2984048487d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:15:15 +0100 Subject: [PATCH 181/233] Changed RSSI range to 0..255 --- src/drivers/drv_rc_input.h | 2 +- src/modules/px4iofirmware/controls.c | 6 +++--- src/modules/px4iofirmware/sbus.c | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 7b18b5b15b..66771faaa4 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -89,7 +89,7 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; - /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */ + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ int32_t rssi; /** Input source */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 58af77997a..ed29c83394 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -94,7 +94,7 @@ controls_tick() { * other. Don't do that. */ - /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; perf_begin(c_gather_dsm); @@ -108,7 +108,7 @@ controls_tick() { else r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; - rssi = 1000; + rssi = 255; } perf_end(c_gather_dsm); @@ -129,7 +129,7 @@ controls_tick() { if (ppm_updated) { /* XXX sample RSSI properly here */ - rssi = 1000; + rssi = 255; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 388502b403..3dcfe7f5b2 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -280,7 +280,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) } - *rssi = 1000; + *rssi = 255; return true; } From 05ec96b0f74b5d6011b683e747aae3bc37cbfbb9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 4 Nov 2013 14:05:52 +0100 Subject: [PATCH 182/233] Startup script for simple logging --- ROMFS/px4fmu_common/init.d/800_sdlogger | 60 +++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 +++ 2 files changed, 66 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger new file mode 100644 index 0000000000..955fe0e2ad --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -0,0 +1,60 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 init to log only + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param save +fi + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + commander start + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +sh /etc/init.d/rc.sensors + +gps start + +attitude_estimator_ekf start + +position_estimator_inav start + +if [ -d /fs/microsd ] +then + if [ $BOARD == fmuv1 ] + then + sdlog2 start -r 50 -e -b 16 + else + sdlog2 start -r 200 -e -b 16 + fi +fi + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f551c1aa8c..06102b707e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -345,6 +345,12 @@ then set MODE custom fi + if param compare SYS_AUTOSTART 800 + then + sh /etc/init.d/800_sdlogger + set MODE custom + fi + # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then From db46672bc4bfc4956baeb3f4d15d2fccf0ef3377 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 12:02:57 +0100 Subject: [PATCH 183/233] Paranoid PPM shape checking --- src/drivers/stm32/drv_hrt.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 5bfbe04b8a..a6b005d274 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -336,17 +336,18 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */ +# define PPM_MIN_PULSE_WIDTH 200 /* minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /* maximum width of a valid first pulse */ # define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ -# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */ +# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +# define PPM_MIN_START 2300 /* shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 #define PPM_MAX_CHANNELS 20 /* Number of same-sized frames required to 'lock' */ -#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ +#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; __EXPORT uint16_t ppm_frame_length = 0; @@ -518,8 +519,8 @@ hrt_ppm_decode(uint32_t status) case ARM: /* we expect a pulse giving us the first mark */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too short or too long */ /* record the mark timing, expect an inactive edge */ ppm.last_mark = ppm.last_edge; @@ -533,8 +534,8 @@ hrt_ppm_decode(uint32_t status) case INACTIVE: /* we expect a short pulse */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too short or too long */ /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; From d5c857615b8714a49d09ae8410b0ae8d6be4a1be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 12:14:15 +0100 Subject: [PATCH 184/233] Pure formatting cleanup of drv_hrt.c, no code / functionality changes --- src/drivers/stm32/drv_hrt.c | 76 +++++++++++++++++++------------------ 1 file changed, 39 insertions(+), 37 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index a6b005d274..b7c9b89a45 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -168,7 +168,7 @@ # error HRT_TIMER_CLOCK must be greater than 1MHz #endif -/* +/** * Minimum/maximum deadlines. * * These are suitable for use with a 16-bit timer/counter clocked @@ -276,7 +276,7 @@ static void hrt_call_invoke(void); * Specific registers and bits used by PPM sub-functions */ #ifdef HRT_PPM_CHANNEL -/* +/* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. * * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). @@ -336,18 +336,18 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MIN_PULSE_WIDTH 200 /* minimum width of a valid first pulse */ -# define PPM_MAX_PULSE_WIDTH 600 /* maximum width of a valid first pulse */ -# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ -# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2300 /* shortest valid start gap (only 2nd part of pulse) */ +# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */ +# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 #define PPM_MAX_CHANNELS 20 -/* Number of same-sized frames required to 'lock' */ -#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ +/** Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; __EXPORT uint16_t ppm_frame_length = 0; @@ -364,12 +364,12 @@ unsigned ppm_pulse_next; static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; -/* PPM decoder state machine */ +/** PPM decoder state machine */ struct { - uint16_t last_edge; /* last capture time */ - uint16_t last_mark; /* last significant edge */ - uint16_t frame_start; /* the frame width */ - unsigned next_channel; /* next channel index */ + uint16_t last_edge; /**< last capture time */ + uint16_t last_mark; /**< last significant edge */ + uint16_t frame_start; /**< the frame width */ + unsigned next_channel; /**< next channel index */ enum { UNSYNCH = 0, ARM, @@ -391,7 +391,7 @@ static void hrt_ppm_decode(uint32_t status); # define CCER_PPM 0 #endif /* HRT_PPM_CHANNEL */ -/* +/** * Initialise the timer we are going to use. * * We expect that we'll own one of the reduced-function STM32 general @@ -437,7 +437,7 @@ hrt_tim_init(void) } #ifdef HRT_PPM_CHANNEL -/* +/** * Handle the PPM decoder state machine. */ static void @@ -577,7 +577,7 @@ error: } #endif /* HRT_PPM_CHANNEL */ -/* +/** * Handle the compare interupt by calling the callout dispatcher * and then re-scheduling the next deadline. */ @@ -606,6 +606,7 @@ hrt_tim_isr(int irq, void *context) hrt_ppm_decode(status); } + #endif /* was this a timer tick? */ @@ -624,7 +625,7 @@ hrt_tim_isr(int irq, void *context) return OK; } -/* +/** * Fetch a never-wrapping absolute time value in microseconds from * some arbitrary epoch shortly after system start. */ @@ -671,7 +672,7 @@ hrt_absolute_time(void) return abstime; } -/* +/** * Convert a timespec to absolute time */ hrt_abstime @@ -685,7 +686,7 @@ ts_to_abstime(struct timespec *ts) return result; } -/* +/** * Convert absolute time to a timespec. */ void @@ -696,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) ts->tv_nsec = abstime * 1000; } -/* +/** * Compare a time value with the current time. */ hrt_abstime @@ -711,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then) return delta; } -/* +/** * Store the absolute time in an interrupt-safe fashion */ hrt_abstime @@ -726,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now) return ts; } -/* +/** * Initalise the high-resolution timing module. */ void @@ -741,7 +742,7 @@ hrt_init(void) #endif } -/* +/** * Call callout(arg) after interval has elapsed. */ void @@ -754,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v arg); } -/* +/** * Call callout(arg) at calltime. */ void @@ -763,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v hrt_call_internal(entry, calltime, 0, callout, arg); } -/* +/** * Call callout(arg) every period. */ void @@ -782,13 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ - /* note that we are using a potentially uninitialised - entry->link here, but it is safe as sq_rem() doesn't - dereference the passed node unless it is found in the - list. So we potentially waste a bit of time searching the - queue for the uninitialised entry->link but we don't do - anything actually unsafe. - */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); @@ -802,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqrestore(flags); } -/* +/** * If this returns true, the call has been invoked and removed from the callout list. * * Always returns false for repeating callouts. @@ -813,7 +814,7 @@ hrt_called(struct hrt_call *entry) return (entry->deadline == 0); } -/* +/** * Remove the entry from the callout list. */ void @@ -896,17 +897,18 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { // re-check call->deadline to allow for - // callouts to re-schedule themselves + // callouts to re-schedule themselves // using hrt_call_delay() if (call->deadline <= now) { call->deadline = deadline + call->period; } + hrt_call_enter(call); } } } -/* +/** * Reschedule the next timer interrupt. * * This routine must be called with interrupts disabled. From c5ef295f68fc475e053d9ab368881743c22f1667 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 15:46:50 +0100 Subject: [PATCH 185/233] Hotfix: Reduce mag influence on att estimate --- .../attitude_estimator_ekf_params.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 52dac652be..e1280445b5 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); -/* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); +/* accel measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); +/* mag measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); +/* offset estimation - UNUSED */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); From dd5549da46eb2914b8e710cd656ec0f44c7ce892 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 15:50:28 +0100 Subject: [PATCH 186/233] Hotfix: Better dead zone defaults --- src/modules/sensors/sensor_params.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 78d4b410a8..f826470600 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC9_MIN, 1000); PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); From a9ea39054dbd6eb62fb3185465848a485c32a046 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 21:19:04 +0100 Subject: [PATCH 187/233] Working around creating an error condition with more than 8 raw RC channels --- src/drivers/px4io/px4io.cpp | 10 +++------- src/modules/px4iofirmware/registers.c | 2 +- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9812ea497d..22d7072330 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1095,8 +1095,10 @@ PX4IO::io_set_rc_config() * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical * controls. */ + + /* fill the mapping with an error condition triggering value */ for (unsigned i = 0; i < _max_rc_input; i++) - input_map[i] = -1; + input_map[i] = UINT16_MAX; /* * NOTE: The indices for mapped channels are 1-based @@ -1128,12 +1130,6 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 4; - ichan = 5; - - for (unsigned i = 0; i < _max_rc_input; i++) - if (input_map[i] == -1) - input_map[i] = ichan++; - /* * Iterate all possible RC inputs. */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7ef1aa3091..b0922f4d69 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -602,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } From 965a7a4f0313ccfe67e6c0f84d76ff3350602fb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 21:33:19 +0100 Subject: [PATCH 188/233] Allow to disable a channel --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/registers.c | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 22d7072330..cac7bf6085 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1098,7 +1098,7 @@ PX4IO::io_set_rc_config() /* fill the mapping with an error condition triggering value */ for (unsigned i = 0; i < _max_rc_input; i++) - input_map[i] = UINT16_MAX; + input_map[i] = UINT8_MAX; /* * NOTE: The indices for mapped channels are 1-based diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index b0922f4d69..0358725db3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -584,6 +584,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint8_t count = 0; + bool disabled = false; /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { @@ -602,7 +603,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } @@ -610,7 +614,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (count) { isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else { + } else if (!disabled) { conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } } From f44f738f0ade4c5ba60a64aa92a2b4447c6f1d1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 21:56:54 +0100 Subject: [PATCH 189/233] Fix return value --- src/drivers/px4io/px4io.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cac7bf6085..0a979267db 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -850,7 +850,7 @@ PX4IO::task_main() /* we're not nice to the lower-priority control groups and only check them when the primary group updated (which is now). */ - io_set_control_groups(); + (void)io_set_control_groups(); } if (now >= poll_last + IO_POLL_INTERVAL) { @@ -962,14 +962,14 @@ out: int PX4IO::io_set_control_groups() { - bool attitude_ok = io_set_control_state(0); + ret = io_set_control_state(0); /* send auxiliary control groups */ (void)io_set_control_state(1); (void)io_set_control_state(2); (void)io_set_control_state(3); - return attitude_ok; + return ret; } int From a4a5eee08dc1ac1c2c68a4981a2299829aeceea0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 23:27:25 +0100 Subject: [PATCH 190/233] Attitude_estimator_ekf: Fix params, this bug caused the multirotor_att_control to stop --- .../attitude_estimator_ekf_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index e1280445b5..3cfddf28ed 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -74,10 +74,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q3 = param_find("EKF_ATT_V3_Q3"); h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V3_R0"); - h->r1 = param_find("EKF_ATT_V3_R1"); - h->r2 = param_find("EKF_ATT_V3_R2"); - h->r3 = param_find("EKF_ATT_V3_R3"); + h->r0 = param_find("EKF_ATT_V4_R0"); + h->r1 = param_find("EKF_ATT_V4_R1"); + h->r2 = param_find("EKF_ATT_V4_R2"); + h->r3 = param_find("EKF_ATT_V4_R3"); h->roll_off = param_find("ATT_ROLL_OFF3"); h->pitch_off = param_find("ATT_PITCH_OFF3"); From 020c47b59ff92f03f3bd574af29f4b217a302626 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 23:57:24 +0100 Subject: [PATCH 191/233] PX4IO driver: even compiling now --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0a979267db..cbdd5acc44 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -962,7 +962,7 @@ out: int PX4IO::io_set_control_groups() { - ret = io_set_control_state(0); + int ret = io_set_control_state(0); /* send auxiliary control groups */ (void)io_set_control_state(1); From d9f75a751b72c266ef68de4e1b7c56fafe1c8d5a Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 28 Dec 2013 08:56:58 +0100 Subject: [PATCH 192/233] Startup script for Wing Wing (Z-84) --- ROMFS/px4fmu_common/init.d/33_io_wingwing | 91 +++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 ++ 2 files changed, 97 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/33_io_wingwing diff --git a/ROMFS/px4fmu_common/init.d/33_io_wingwing b/ROMFS/px4fmu_common/init.d/33_io_wingwing new file mode 100644 index 0000000000..fd709ec864 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/33_io_wingwing @@ -0,0 +1,91 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_T_TIME_CONST 4 + param set FW_Y_ROLLFF 2.0 + param set FW_L1_PERIOD 25.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] +then + exit +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 06102b707e..3408b4ad53 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -321,6 +321,12 @@ then set MODE custom fi + if param compare SYS_AUTOSTART 33 + then + sh /etc/init.d/32_io_wingwing + set MODE custom + fi + if param compare SYS_AUTOSTART 40 then sh /etc/init.d/40_io_segway From f17538a7f356b9db257ca6f36b8f76c47eafa328 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 28 Dec 2013 09:03:42 +0100 Subject: [PATCH 193/233] Updated parameters to latest flight tested values --- ROMFS/px4fmu_common/init.d/33_io_wingwing | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/33_io_wingwing b/ROMFS/px4fmu_common/init.d/33_io_wingwing index fd709ec864..538c69711d 100644 --- a/ROMFS/px4fmu_common/init.d/33_io_wingwing +++ b/ROMFS/px4fmu_common/init.d/33_io_wingwing @@ -11,9 +11,10 @@ then param set FW_AIRSPD_MIN 7 param set FW_AIRSPD_TRIM 9 param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 param set FW_P_D 0 param set FW_P_I 0 - param set FW_P_IMAX 15 + param set FW_P_IMAX 20 param set FW_P_LIM_MAX 30 param set FW_P_LIM_MIN -20 param set FW_P_P 30 @@ -28,11 +29,10 @@ then param set FW_THR_CRUISE 0.65 param set FW_THR_MAX 0.7 param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_T_TIME_CONST 4 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 param set FW_Y_ROLLFF 2.0 - param set FW_L1_PERIOD 25.0 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 From 4e5f743e4184269b05bb037d4787665afe996ab8 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 28 Dec 2013 14:45:29 +0100 Subject: [PATCH 194/233] Added config for the FX-79 --- ROMFS/px4fmu_common/init.d/34_io_fx79 | 87 +++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 ++ ROMFS/px4fmu_common/mixers/FMU_FX79.mix | 72 ++++++++++++++++++++ 3 files changed, 165 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/34_io_fx79 create mode 100755 ROMFS/px4fmu_common/mixers/FMU_FX79.mix diff --git a/ROMFS/px4fmu_common/init.d/34_io_fx79 b/ROMFS/px4fmu_common/init.d/34_io_fx79 new file mode 100644 index 0000000000..eb46ad8a7e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/34_io_fx79 @@ -0,0 +1,87 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 17 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix +else + echo "Using /etc/mixers/FMU_FX79.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3408b4ad53..9a258e14d0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -327,6 +327,12 @@ then set MODE custom fi + if param compare SYS_AUTOSTART 34 + then + sh /etc/init.d/34_io_fx79 + set MODE custom + fi + if param compare SYS_AUTOSTART 40 then sh /etc/init.d/40_io_segway diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix new file mode 100755 index 0000000000..0a1dca98d5 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix @@ -0,0 +1,72 @@ +FX-79 Delta-wing mixer for PX4FMU +================================= + +Designed for FX-79. + +TODO (sjwilks): Add mixers for flaps. + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 5000 8000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 8000 5000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 3f84ad79c58bdc208a18b329580940a7c9438447 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 28 Dec 2013 14:46:55 +0100 Subject: [PATCH 195/233] Typo fix. --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3408b4ad53..0d814848c6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -323,7 +323,7 @@ then if param compare SYS_AUTOSTART 33 then - sh /etc/init.d/32_io_wingwing + sh /etc/init.d/33_io_wingwing set MODE custom fi From 1637c62751dfed995f80c538d1c6d0af011c399a Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 28 Dec 2013 17:19:09 +0100 Subject: [PATCH 196/233] Set the tested defaults --- ROMFS/px4fmu_common/init.d/34_io_fx79 | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/34_io_fx79 b/ROMFS/px4fmu_common/init.d/34_io_fx79 index eb46ad8a7e..827f3b1539 100644 --- a/ROMFS/px4fmu_common/init.d/34_io_fx79 +++ b/ROMFS/px4fmu_common/init.d/34_io_fx79 @@ -8,6 +8,10 @@ echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -20,15 +24,15 @@ then param set FW_R_D 0 param set FW_R_I 5 param set FW_R_IMAX 20 - param set FW_R_P 100 + param set FW_R_P 80 param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 + param set FW_THR_CRUISE 0.75 param set FW_THR_MAX 1 param set FW_THR_MIN 0 param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 17 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 From 1cca94defc2fb8d18dd2f558d0181279f701f077 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 28 Dec 2013 17:23:52 +0100 Subject: [PATCH 197/233] Whitespace fix --- ROMFS/px4fmu_common/init.d/34_io_fx79 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/34_io_fx79 b/ROMFS/px4fmu_common/init.d/34_io_fx79 index 827f3b1539..9892049526 100644 --- a/ROMFS/px4fmu_common/init.d/34_io_fx79 +++ b/ROMFS/px4fmu_common/init.d/34_io_fx79 @@ -31,8 +31,8 @@ then param set FW_THR_MIN 0 param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 From b20e07a1c5866e8222065c7e8848b78a052d703c Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 28 Dec 2013 18:12:55 +0100 Subject: [PATCH 198/233] Updated defaults following recent test flight that included autostart and land --- ROMFS/px4fmu_common/init.d/31_io_phantom | 33 ++++++++++++++---------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 63cd7f9b2e..62cfe1a9c6 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -8,30 +8,35 @@ echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 param set FW_P_P 60 param set FW_P_RMAX_NEG 0 param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 + param set FW_P_ROLLFF 2 param set FW_R_D 0 param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 param set FW_THR_MAX 1 - param set FW_THR_MIN 0 + param set FW_THR_MIN 0.5 param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 17 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 + param set SYS_AUTOCONFIG 0 param save fi From 01be817c5993d635d382cd5664c77e7f9728bd3f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Dec 2013 22:14:28 +0100 Subject: [PATCH 199/233] Allow N comparisons of a param value, returns success if one matches --- src/systemcmds/param/param.c | 35 +++++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 40a9297a77..65f291f401 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2012, 2013 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 @@ -63,7 +62,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val); -static void do_compare(const char* name, const char* val); +static void do_compare(const char* name, const char* vals[], unsigned comparisons); int param_main(int argc, char *argv[]) @@ -121,7 +120,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "compare")) { if (argc >= 4) { - do_compare(argv[2], argv[3]); + do_compare(argv[2], &argv[3], argc - 3); } else { errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); } @@ -306,7 +305,7 @@ do_set(const char* name, const char* val) } static void -do_compare(const char* name, const char* val) +do_compare(const char* name, const char* vals[], unsigned comparisons) { int32_t i; float f; @@ -330,12 +329,16 @@ do_compare(const char* name, const char* val) /* convert string */ char* end; - int j = strtol(val,&end,10); - if (i == j) { - printf(" %d: ", i); - ret = 0; - } + for (unsigned k = 0; k < comparisons; k++) { + + int j = strtol(vals[k],&end,10); + + if (i == j) { + printf(" %d: ", i); + ret = 0; + } + } } break; @@ -345,10 +348,14 @@ do_compare(const char* name, const char* val) /* convert string */ char* end; - float g = strtod(val, &end); - if (fabsf(f - g) < 1e-7f) { - printf(" %4.4f: ", (double)f); - ret = 0; + + for (unsigned k = 0; k < comparisons; k++) { + + float g = strtod(vals[k], &end); + if (fabsf(f - g) < 1e-7f) { + printf(" %4.4f: ", (double)f); + ret = 0; + } } } @@ -359,7 +366,7 @@ do_compare(const char* name, const char* val) } if (ret == 0) { - printf("%c %s: equal\n", + printf("%c %s: match\n", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); } From d7a3aaba45518e04e362101a0e81e55462421375 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Dec 2013 22:16:06 +0100 Subject: [PATCH 200/233] Getting multicopter startup back to generic, trimming down number and content of different startup scripts --- ROMFS/px4fmu_common/init.d/rc.mc_interface | 49 ++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_interface diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface new file mode 100644 index 0000000000..6bb2e84ecf --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -0,0 +1,49 @@ +#!nsh +# +# Script to set PWM min / max limits and mixer +# + +# +# Load mixer +# +if [ $FRAME_GEOMETRY == x ] +then + echo "Frame geometry X" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + if [ $FRAME_GEOMETRY == w ] + then + echo "Frame geometry W" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + else + echo "Frame geometry +" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + fi +fi + +if [ $FRAME_COUNT == 4 ] +then + set OUTPUTS 1234 + param set MAV_TYPE 2 +else + if [ $FRAME_COUNT == 6 ] + then + set OUTPUTS 123456 + param set MAV_TYPE 13 + else + set OUTPUTS 12345678 + fi +fi + + +# +# Set PWM output frequency +# +pwm rate -c $OUTPUTS -r $PWM_RATE + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c $OUTPUTS -p $PWM_DISARMED +pwm min -c $OUTPUTS -p $PWM_MIN +pwm max -c $OUTPUTS -p $PWM_MAX From 501c5ff49f2ba05d21cfe10775c1ebc3bc6af2c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Dec 2013 22:18:07 +0100 Subject: [PATCH 201/233] Cleaned up startup, should be completely compatible, but allows clean QGC indices --- ROMFS/px4fmu_common/init.d/#!nsh | 4 + .../{15_tbs_discovery => 10015_tbs_discovery} | 7 -- .../init.d/{16_3dr_iris => 10016_3dr_iris} | 7 -- .../{100_mpx_easystar => 2100_mpx_easystar} | 7 -- .../init.d/{101_hk_bixler => 2101_hk_bixler} | 7 -- .../{102_3dr_skywalker => 2102_3dr_skywalker} | 7 -- .../{30_io_camflyer => 3030_io_camflyer} | 7 -- .../init.d/{31_io_phantom => 3031_io_phantom} | 7 -- .../{32_skywalker_x5 => 3032_skywalker_x5} | 7 -- .../{33_io_wingwing => 3033_io_wingwing} | 7 -- .../init.d/{34_io_fx79 => 3034_io_fx79} | 7 -- .../init.d/{08_ardrone => 4008_ardrone} | 0 .../{09_ardrone_flow => 4009_ardrone_flow} | 0 .../init.d/{10_dji_f330 => 4010_dji_f330} | 30 +----- .../init.d/{11_dji_f450 => 4011_dji_f450} | 7 -- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 7 -- ROMFS/px4fmu_common/init.d/800_sdlogger | 7 -- ROMFS/px4fmu_common/init.d/rcS | 96 ++++++++++++------- 18 files changed, 65 insertions(+), 156 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/#!nsh rename ROMFS/px4fmu_common/init.d/{15_tbs_discovery => 10015_tbs_discovery} (95%) rename ROMFS/px4fmu_common/init.d/{16_3dr_iris => 10016_3dr_iris} (95%) rename ROMFS/px4fmu_common/init.d/{100_mpx_easystar => 2100_mpx_easystar} (96%) rename ROMFS/px4fmu_common/init.d/{101_hk_bixler => 2101_hk_bixler} (96%) rename ROMFS/px4fmu_common/init.d/{102_3dr_skywalker => 2102_3dr_skywalker} (96%) rename ROMFS/px4fmu_common/init.d/{30_io_camflyer => 3030_io_camflyer} (94%) rename ROMFS/px4fmu_common/init.d/{31_io_phantom => 3031_io_phantom} (96%) rename ROMFS/px4fmu_common/init.d/{32_skywalker_x5 => 3032_skywalker_x5} (94%) rename ROMFS/px4fmu_common/init.d/{33_io_wingwing => 3033_io_wingwing} (96%) rename ROMFS/px4fmu_common/init.d/{34_io_fx79 => 3034_io_fx79} (96%) rename ROMFS/px4fmu_common/init.d/{08_ardrone => 4008_ardrone} (100%) rename ROMFS/px4fmu_common/init.d/{09_ardrone_flow => 4009_ardrone_flow} (100%) rename ROMFS/px4fmu_common/init.d/{10_dji_f330 => 4010_dji_f330} (76%) rename ROMFS/px4fmu_common/init.d/{11_dji_f450 => 4011_dji_f450} (95%) diff --git a/ROMFS/px4fmu_common/init.d/#!nsh b/ROMFS/px4fmu_common/init.d/#!nsh new file mode 100644 index 0000000000..69b7182828 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/#!nsh @@ -0,0 +1,4 @@ +#!nsh +# +# Script to set PWM min / max limits and mixer +# \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery similarity index 95% rename from ROMFS/px4fmu_common/init.d/15_tbs_discovery rename to ROMFS/px4fmu_common/init.d/10015_tbs_discovery index c79e9d2830..81d4b5d570 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -32,8 +32,6 @@ fi # param set MAV_TYPE 2 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -74,8 +72,3 @@ pwm max -c 1234 -p 1900 # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris similarity index 95% rename from ROMFS/px4fmu_common/init.d/16_3dr_iris rename to ROMFS/px4fmu_common/init.d/10016_3dr_iris index f6b071cf1b..b0f4eda79e 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -32,8 +32,6 @@ fi # param set MAV_TYPE 2 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -73,8 +71,3 @@ pwm min -c 1234 -p 1050 # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar similarity index 96% rename from ROMFS/px4fmu_common/init.d/100_mpx_easystar rename to ROMFS/px4fmu_common/init.d/2100_mpx_easystar index abe378b22f..97c2d7c903 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -42,8 +42,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -80,8 +78,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler similarity index 96% rename from ROMFS/px4fmu_common/init.d/101_hk_bixler rename to ROMFS/px4fmu_common/init.d/2101_hk_bixler index c616da9886..995d3ba074 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -42,8 +42,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -80,8 +78,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker similarity index 96% rename from ROMFS/px4fmu_common/init.d/102_3dr_skywalker rename to ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index e5d21c3219..a6d2ace962 100644 --- a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -42,8 +42,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -82,8 +80,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer similarity index 94% rename from ROMFS/px4fmu_common/init.d/30_io_camflyer rename to ROMFS/px4fmu_common/init.d/3030_io_camflyer index 8a8bc15903..65f01c9743 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -20,8 +20,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -58,8 +56,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom similarity index 96% rename from ROMFS/px4fmu_common/init.d/31_io_phantom rename to ROMFS/px4fmu_common/init.d/3031_io_phantom index 62cfe1a9c6..0cf6ee39a5 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_io_phantom @@ -47,8 +47,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -85,8 +83,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 similarity index 94% rename from ROMFS/px4fmu_common/init.d/32_skywalker_x5 rename to ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 1e752f13a2..41e0416548 100644 --- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -20,8 +20,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -58,8 +56,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/33_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing similarity index 96% rename from ROMFS/px4fmu_common/init.d/33_io_wingwing rename to ROMFS/px4fmu_common/init.d/3033_io_wingwing index 538c69711d..82ff425e6d 100644 --- a/ROMFS/px4fmu_common/init.d/33_io_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_io_wingwing @@ -46,8 +46,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -84,8 +82,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/34_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 similarity index 96% rename from ROMFS/px4fmu_common/init.d/34_io_fx79 rename to ROMFS/px4fmu_common/init.d/3034_io_fx79 index 9892049526..759c17bb44 100644 --- a/ROMFS/px4fmu_common/init.d/34_io_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_io_fx79 @@ -46,8 +46,6 @@ fi # param set MAV_TYPE 1 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -84,8 +82,3 @@ fi # Start common fixedwing apps # sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone similarity index 100% rename from ROMFS/px4fmu_common/init.d/08_ardrone rename to ROMFS/px4fmu_common/init.d/4008_ardrone diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow similarity index 100% rename from ROMFS/px4fmu_common/init.d/09_ardrone_flow rename to ROMFS/px4fmu_common/init.d/4009_ardrone_flow diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 similarity index 76% rename from ROMFS/px4fmu_common/init.d/10_dji_f330 rename to ROMFS/px4fmu_common/init.d/4010_dji_f330 index 467b56bbf9..7054210e24 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -41,14 +41,6 @@ then param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no # # Start and configure PX4IO or FMU interface @@ -69,29 +61,9 @@ else set EXIT_ON_END yes fi -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +sh /etc/init.d/rc.mc_interface # # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 similarity index 95% rename from ROMFS/px4fmu_common/init.d/11_dji_f450 rename to ROMFS/px4fmu_common/init.d/4011_dji_f450 index 818f9375e4..84e48696ae 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -32,8 +32,6 @@ fi # param set MAV_TYPE 2 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -76,8 +74,3 @@ pwm max -c 1234 -p 1800 # Start common multirotor apps # sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index 1ee84b9b01..acd8027fb5 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -34,8 +34,6 @@ fi # param set MAV_TYPE 2 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -76,8 +74,3 @@ pwm max -c 1234 -p 1900 # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger index 955fe0e2ad..1198b42cd3 100644 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -13,8 +13,6 @@ then param save fi -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -53,8 +51,3 @@ then sdlog2 start -r 200 -e -b 16 fi fi - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e6a748f8f9..8bab93fbc0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -194,64 +194,83 @@ then tone_alarm MNGGG fi fi + + set EXIT_ON_END no # # Check if auto-setup from one of the standard scripts is wanted # SYS_AUTOSTART = 0 means no autostart (default) # + # AUTOSTART PARTITION: + # 0 .. 999 Reserved (historical) + # 1000 .. 1999 Simulation setups + # 2000 .. 2999 Standard planes + # 3000 .. 3999 Flying wing + # 4000 .. 4999 Quad X + # 5000 .. 5999 Quad + + # 6000 .. 6999 Hexa X + # 7000 .. 7999 Hexa + + # 8000 .. 8999 Octo X + # 9000 .. 9999 Octo + + # 10000 .. 19999 Wide arm / H frame - if param compare SYS_AUTOSTART 8 + if param compare SYS_AUTOSTART 4008 8 then - sh /etc/init.d/08_ardrone + sh /etc/init.d/4008_ardrone set MODE custom fi - if param compare SYS_AUTOSTART 9 + if param compare SYS_AUTOSTART 4009 9 then - sh /etc/init.d/09_ardrone_flow + sh /etc/init.d/4009_ardrone_flow set MODE custom fi - if param compare SYS_AUTOSTART 10 + if param compare SYS_AUTOSTART 4010 10 then - sh /etc/init.d/10_dji_f330 + set FRAME_GEOMETRY x + set FRAME_COUNT 4 + set PWM_MIN 1200 + set PWM_MAX 1900 + set PWM_DISARMED 900 + sh /etc/init.d/4010_dji_f330 set MODE custom fi - if param compare SYS_AUTOSTART 11 + if param compare SYS_AUTOSTART 4011 11 then - sh /etc/init.d/11_dji_f450 + sh /etc/init.d/4011_dji_f450 set MODE custom fi - if param compare SYS_AUTOSTART 12 + if param compare SYS_AUTOSTART 6012 12 then set MIXER /etc/mixers/FMU_hex_x.mix sh /etc/init.d/12-13_hex set MODE custom fi - if param compare SYS_AUTOSTART 13 + if param compare SYS_AUTOSTART 6013 13 then set MIXER /etc/mixers/FMU_hex_+.mix sh /etc/init.d/12-13_hex set MODE custom fi - if param compare SYS_AUTOSTART 15 + if param compare SYS_AUTOSTART 10015 15 then - sh /etc/init.d/15_tbs_discovery + sh /etc/init.d/10015_tbs_discovery set MODE custom fi - if param compare SYS_AUTOSTART 16 + if param compare SYS_AUTOSTART 10016 16 then - sh /etc/init.d/16_3dr_iris + sh /etc/init.d/10016_3dr_iris set MODE custom fi # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 17 + if param compare SYS_AUTOSTART 4017 17 then set MKBLCTRL_MODE no set MKBLCTRL_FRAME x @@ -260,7 +279,7 @@ then fi # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 18 + if param compare SYS_AUTOSTART 5018 18 then set MKBLCTRL_MODE no set MKBLCTRL_FRAME + @@ -269,7 +288,7 @@ then fi # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 19 + if param compare SYS_AUTOSTART 4019 19 then set MKBLCTRL_MODE yes set MKBLCTRL_FRAME x @@ -278,7 +297,7 @@ then fi # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 20 + if param compare SYS_AUTOSTART 5020 20 then set MKBLCTRL_MODE yes set MKBLCTRL_FRAME + @@ -287,7 +306,7 @@ then fi # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 21 + if param compare SYS_AUTOSTART 4021 21 then set FRAME_GEOMETRY x set ESC_MAKER afro @@ -296,40 +315,40 @@ then fi # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 22 + if param compare SYS_AUTOSTART 10022 22 then set FRAME_GEOMETRY w sh /etc/init.d/rc.custom_io_esc set MODE custom fi - if param compare SYS_AUTOSTART 30 + if param compare SYS_AUTOSTART 3030 30 then - sh /etc/init.d/30_io_camflyer + sh /etc/init.d/3030_io_camflyer set MODE custom fi - if param compare SYS_AUTOSTART 31 + if param compare SYS_AUTOSTART 3031 31 then - sh /etc/init.d/31_io_phantom + sh /etc/init.d/3031_io_phantom set MODE custom fi - if param compare SYS_AUTOSTART 32 + if param compare SYS_AUTOSTART 3032 32 then - sh /etc/init.d/32_skywalker_x5 + sh /etc/init.d/3032_skywalker_x5 set MODE custom fi - if param compare SYS_AUTOSTART 33 + if param compare SYS_AUTOSTART 3033 33 then - sh /etc/init.d/33_io_wingwing + sh /etc/init.d/3033_io_wingwing set MODE custom fi - if param compare SYS_AUTOSTART 34 + if param compare SYS_AUTOSTART 3034 34 then - sh /etc/init.d/34_io_fx79 + sh /etc/init.d/3034_io_fx79 set MODE custom fi @@ -339,21 +358,21 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 100 + if param compare SYS_AUTOSTART 2100 100 then - sh /etc/init.d/100_mpx_easystar + sh /etc/init.d/2100_mpx_easystar set MODE custom fi - if param compare SYS_AUTOSTART 101 + if param compare SYS_AUTOSTART 2101 101 then - sh /etc/init.d/101_hk_bixler + sh /etc/init.d/2101_hk_bixler set MODE custom fi - if param compare SYS_AUTOSTART 102 + if param compare SYS_AUTOSTART 2102 102 then - sh /etc/init.d/102_3dr_skywalker + sh /etc/init.d/2102_3dr_skywalker set MODE custom fi @@ -402,5 +421,10 @@ then fi + if [ $EXIT_ON_END == yes ] + then + exit + fi + # End of autostart fi From ab330de180c9f47c1d6fec54cc29c98be8c56f4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Dec 2013 22:34:09 +0100 Subject: [PATCH 202/233] Fixed typo --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8bab93fbc0..3b780f9e75 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -250,7 +250,7 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 6013 13 + if param compare SYS_AUTOSTART 7013 13 then set MIXER /etc/mixers/FMU_hex_+.mix sh /etc/init.d/12-13_hex From 5e8857e411c4d016ffef1f2333d7728e4958c523 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Dec 2013 22:52:23 +0100 Subject: [PATCH 203/233] Further cleanup, added octos --- ROMFS/px4fmu_common/init.d/#!nsh | 4 - .../init.d/{12-13_hex => rc.hexa} | 2 +- ROMFS/px4fmu_common/init.d/rc.octo | 98 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 24 ++++- 4 files changed, 121 insertions(+), 7 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/#!nsh rename ROMFS/px4fmu_common/init.d/{12-13_hex => rc.hexa} (97%) create mode 100644 ROMFS/px4fmu_common/init.d/rc.octo diff --git a/ROMFS/px4fmu_common/init.d/#!nsh b/ROMFS/px4fmu_common/init.d/#!nsh deleted file mode 100644 index 69b7182828..0000000000 --- a/ROMFS/px4fmu_common/init.d/#!nsh +++ /dev/null @@ -1,4 +0,0 @@ -#!nsh -# -# Script to set PWM min / max limits and mixer -# \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/rc.hexa similarity index 97% rename from ROMFS/px4fmu_common/init.d/12-13_hex rename to ROMFS/px4fmu_common/init.d/rc.hexa index a7578bcaf8..c49de3e0d8 100644 --- a/ROMFS/px4fmu_common/init.d/12-13_hex +++ b/ROMFS/px4fmu_common/init.d/rc.hexa @@ -73,7 +73,7 @@ fi # # Load mixer # -mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix +mixer load /dev/pwm_output $MIXER # # Set PWM output frequency to 400 Hz diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo new file mode 100644 index 0000000000..2c50d266cb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.octo @@ -0,0 +1,98 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Hex" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ +# 13 = hexarotor +# +param set MAV_TYPE 13 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output $MIXER + +# +# Set PWM output frequency to 400 Hz +# +pwm rate -a -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 12345678 -p 900 +pwm min -c 12345678 -p 1100 +pwm max -c 12345678 -p 1900 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3b780f9e75..05657b03ef 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -243,17 +243,37 @@ then set MODE custom fi + if param compare SYS_AUTOSTART 4012 + then + sh /etc/init.d/666_fmu_q_x550 + set MODE custom + fi + if param compare SYS_AUTOSTART 6012 12 then set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/12-13_hex + sh /etc/init.d/rc.hexa set MODE custom fi if param compare SYS_AUTOSTART 7013 13 then set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/12-13_hex + sh /etc/init.d/rc.hexa + set MODE custom + fi + + if param compare SYS_AUTOSTART 8001 + then + set MIXER /etc/mixers/FMU_octo_x.mix + sh /etc/init.d/rc.octo + set MODE custom + fi + + if param compare SYS_AUTOSTART 9001 + then + set MIXER /etc/mixers/FMU_octo_+.mix + sh /etc/init.d/rc.octo set MODE custom fi From 07c1ff77a236f69b7788e6184eac78efab3aafa7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Dec 2013 23:21:31 +0100 Subject: [PATCH 204/233] Fixed MAV type and typo --- ROMFS/px4fmu_common/init.d/rc.octo | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo index 2c50d266cb..6adfa30716 100644 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ b/ROMFS/px4fmu_common/init.d/rc.octo @@ -1,6 +1,6 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on Hex" +echo "[init] Octorotor startup" # # Load default params for this platform @@ -45,9 +45,9 @@ fi # # Force some key parameters to sane values # MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor +# 14 = octorotor # -param set MAV_TYPE 13 +param set MAV_TYPE 14 set EXIT_ON_END no From 87a61de670190b78331455f3bc84e0612a302f90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Dec 2013 12:02:23 +0100 Subject: [PATCH 205/233] Support for more than 8 output ports --- src/modules/mavlink/orb_listener.c | 34 ++++++++++++++++++------------ 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 9e43467cc3..c1401368d5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -348,20 +348,26 @@ l_input_rc(const struct listener *l) /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); - if (gcs_link) - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, - 0, - (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, - (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, - (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, - (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, - (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, - (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, - (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, - (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, - 255); + if (gcs_link) { + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(chan, + rc_raw.timestamp / 1000, + i, + (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX, + rc_raw.rssi); + } + } } void From 0153e334ff88e9b68425afb8847d32d0c21e73af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Dec 2013 12:03:35 +0100 Subject: [PATCH 206/233] Add note about multi-port support on GCS side --- src/modules/mavlink/orb_listener.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index c1401368d5..92b1b45be7 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -495,7 +495,8 @@ l_actuator_outputs(const struct listener *l) if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - l->arg /* port number */, + l->arg /* port number - needs GCS support */, + /* QGC has port number support already */ act_outputs.output[0], act_outputs.output[1], act_outputs.output[2], From bc76924b7e7703cf7f2167e132c98302a1b79348 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Dec 2013 00:52:50 +0100 Subject: [PATCH 207/233] Cleanup, removed commander start calls in locations where its not needed --- ROMFS/px4fmu_common/init.d/4009_ardrone_flow | 5 ----- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 2 -- ROMFS/px4fmu_common/init.d/40_io_segway | 5 ----- ROMFS/px4fmu_common/init.d/800_sdlogger | 2 -- ROMFS/px4fmu_common/init.d/rc.hexa | 10 +++------- ROMFS/px4fmu_common/init.d/rc.octo | 10 +++------- 6 files changed, 6 insertions(+), 28 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow index 9b739f2455..2886bcb752 100644 --- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow @@ -54,11 +54,6 @@ ardrone_interface start -d /dev/ttyS1 # sh /etc/init.d/rc.sensors -# -# Start the commander. -# -commander start - # # Start the attitude estimator # diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 84e48696ae..a1d253191d 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -41,8 +41,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index fb9dec0437..ad455b4403 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -28,11 +28,6 @@ usleep 5000 # Start and configure PX4IO interface # sh /etc/init.d/rc.io - -# -# Start the commander (depends on orb, mavlink) -# -commander start # # Start the sensors (depends on orb, px4io) diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger index 1198b42cd3..9b90cbdd04 100644 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -22,8 +22,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io # Set PWM values for DJI ESCs else diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa index c49de3e0d8..097db28e48 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ b/ROMFS/px4fmu_common/init.d/rc.hexa @@ -52,7 +52,7 @@ param set MAV_TYPE 13 set EXIT_ON_END no # -# Start and configure PX4IO or FMU interface +# Start and configure PX4IO interface # if px4io detect then @@ -62,12 +62,8 @@ then sh /etc/init.d/rc.io else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # This is not possible on a hexa + tone_alarm error fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo index 6adfa30716..ecb12e96e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ b/ROMFS/px4fmu_common/init.d/rc.octo @@ -52,7 +52,7 @@ param set MAV_TYPE 14 set EXIT_ON_END no # -# Start and configure PX4IO or FMU interface +# Start and configure PX4IO interface # if px4io detect then @@ -62,12 +62,8 @@ then sh /etc/init.d/rc.io else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # This is not possible on an octo + tone_alarm error fi # From 9a54c7c64d84f7fa626f199a1a72031edb2ee72e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Dec 2013 00:53:12 +0100 Subject: [PATCH 208/233] Removed special logging config to cut down build times --- makefiles/config_px4fmu-v2_logging.mk | 157 -------------------------- 1 file changed, 157 deletions(-) delete mode 100644 makefiles/config_px4fmu-v2_logging.mk diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk deleted file mode 100644 index ed90f6464c..0000000000 --- a/makefiles/config_px4fmu-v2_logging.mk +++ /dev/null @@ -1,157 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS, copy the px4iov2 firmware into -# the ROMFS if it's available -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_logging -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4fmu -MODULES += drivers/px4io -MODULES += drivers/boards/px4fmu-v2 -MODULES += drivers/rgbled -MODULES += drivers/mpu6000 -MODULES += drivers/lsm303d -MODULES += drivers/l3gd20 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/mb12xx -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors -MODULES += drivers/blinkm -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# Needs to be burned to the ground and re-written; for now, -# just don't build it. -#MODULES += drivers/mkblctrl - -# -# System commands -# -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard - -# -# Estimation modules (EKF/ SO3 / other filters) -# -MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 -MODULES += modules/att_pos_estimator_ekf -MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator - -# -# Vehicle Control -# -#MODULES += modules/segway # XXX Needs GCC 4.7 fix -MODULES += modules/fw_pos_control_l1 -MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) From 50cbd1949971241a0b6108708b2b3fefe0c498e3 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 30 Dec 2013 20:27:04 +0100 Subject: [PATCH 209/233] Fixes to FrSky telemetry data formats. --- src/drivers/frsky_telemetry/frsky_data.c | 137 +++++++++++++++++------ src/drivers/frsky_telemetry/frsky_data.h | 33 ------ 2 files changed, 105 insertions(+), 65 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 8a57070b17..3a0c4b4b3b 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -51,11 +51,52 @@ #include #include #include +#include + + +// FrSky sensor hub data IDs +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + + +#define frac(f) (f - (int)f) + +float frsky_format_gps(float dec); static int battery_sub = -1; static int sensor_sub = -1; static int global_position_sub = -1; +static int vehicle_status_sub = -1; /** @@ -66,6 +107,7 @@ void frsky_init() battery_sub = orb_subscribe(ORB_ID(battery_status)); global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); } /** @@ -107,13 +149,16 @@ frsky_send_byte(int uart, uint8_t value) * Sends one data id/value pair. */ static void -frsky_send_data(int uart, uint8_t id, uint16_t data) +frsky_send_data(int uart, uint8_t id, int16_t data) { + // Cast data to unsigned, because signed shift might behave incorrectly + uint16_t udata = data; + frsky_send_startstop(uart); frsky_send_byte(uart, id); - frsky_send_byte(uart, data); /* Low */ - frsky_send_byte(uart, data >> 8); /* High */ + frsky_send_byte(uart, udata); /* LSB */ + frsky_send_byte(uart, udata >> 8); /* MSB */ } /** @@ -133,70 +178,96 @@ void frsky_send_frame1(int uart) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* send formatted frame */ - // TODO - frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); - frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius); + frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); frsky_send_data(uart, FRSKY_ID_TEMP2, 0); frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); - frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v); - frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f); + float voltage = battery.voltage_v * 11.0f / 21.0f; + frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage); + frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f); frsky_send_data(uart, FRSKY_ID_RPM, 0); frsky_send_startstop(uart); } +/** + * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + */ +float frsky_format_gps(float dec) +{ + float dms_deg = (int)dec; + float dec_deg = dec - dms_deg; + float dms_min = (int)(dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; + + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); +} + /** * Sends frame 2 (every 1000ms): * course, latitude, longitude, speed, altitude (GPS), fuel level */ void frsky_send_frame2(int uart) { - /* get a local copy of the battery data */ + /* get a local copy of the global position data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + /* send formatted frame */ - // TODO float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + char lat_ns = 0, lon_ew = 0; if (global_pos.valid) { + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - // TODO: latitude, longitude - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - alt = global_pos.alt; + lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; } frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); - frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); - frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, (speed - (int)speed) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); - frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, (alt - (int)alt) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); - frsky_send_data(uart, FRSKY_ID_FUEL, 0); + frsky_send_data(uart, FRSKY_ID_FUEL, vehicle_status.battery_remaining); - frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); frsky_send_startstop(uart); } @@ -213,11 +284,13 @@ void frsky_send_frame3(int uart) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* send formatted frame */ - // TODO - frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0); - frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0); - frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0); - frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); frsky_send_startstop(uart); } diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index e0c39a42bd..cea93ef2b9 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -42,39 +42,6 @@ #ifndef _FRSKY_DATA_H #define _FRSKY_DATA_H -// FrSky sensor hub data IDs -#define FRSKY_ID_GPS_ALT_BP 0x01 -#define FRSKY_ID_TEMP1 0x02 -#define FRSKY_ID_RPM 0x03 -#define FRSKY_ID_FUEL 0x04 -#define FRSKY_ID_TEMP2 0x05 -#define FRSKY_ID_VOLTS 0x06 -#define FRSKY_ID_GPS_ALT_AP 0x09 -#define FRSKY_ID_BARO_ALT_BP 0x10 -#define FRSKY_ID_GPS_SPEED_BP 0x11 -#define FRSKY_ID_GPS_LONG_BP 0x12 -#define FRSKY_ID_GPS_LAT_BP 0x13 -#define FRSKY_ID_GPS_COURS_BP 0x14 -#define FRSKY_ID_GPS_DAY_MONTH 0x15 -#define FRSKY_ID_GPS_YEAR 0x16 -#define FRSKY_ID_GPS_HOUR_MIN 0x17 -#define FRSKY_ID_GPS_SEC 0x18 -#define FRSKY_ID_GPS_SPEED_AP 0x19 -#define FRSKY_ID_GPS_LONG_AP 0x1A -#define FRSKY_ID_GPS_LAT_AP 0x1B -#define FRSKY_ID_GPS_COURS_AP 0x1C -#define FRSKY_ID_BARO_ALT_AP 0x21 -#define FRSKY_ID_GPS_LONG_EW 0x22 -#define FRSKY_ID_GPS_LAT_NS 0x23 -#define FRSKY_ID_ACCEL_X 0x24 -#define FRSKY_ID_ACCEL_Y 0x25 -#define FRSKY_ID_ACCEL_Z 0x26 -#define FRSKY_ID_CURRENT 0x28 -#define FRSKY_ID_VARIO 0x30 -#define FRSKY_ID_VFAS 0x39 -#define FRSKY_ID_VOLTS_BP 0x3A -#define FRSKY_ID_VOLTS_AP 0x3B - // Public functions void frsky_init(void); void frsky_send_frame1(int uart); From 5b302fef59354f536e83a0b14572d2f954a6e682 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 31 Dec 2013 14:47:01 +0100 Subject: [PATCH 210/233] HOTFIX: Increased attitude control updates to 50 Hz - was less than 10 Hz and unintended slow --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 00a0dcd619..60c902ce53 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -479,7 +479,8 @@ FixedwingAttitudeControl::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - orb_set_interval(_att_sub, 100); + /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */ + orb_set_interval(_att_sub, 17); parameters_update(); From 1d9e8a7b721685b9290103d82db04f9b0a587d65 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 23:43:23 +0100 Subject: [PATCH 211/233] Reboot if IO update startup fails on first try --- ROMFS/px4fmu_common/init.d/rcS | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 05657b03ef..f122921c56 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -176,7 +176,7 @@ then tone_alarm MBABGP if px4io forceupdate 14662 $io_file then - usleep 200000 + usleep 500000 if px4io start then echo "PX4IO restart OK" @@ -186,7 +186,8 @@ then echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile tone_alarm MNGGG - sh /etc/init.d/rc.error + sleep 10 + reboot fi else echo "PX4IO update failed" From 5f44be31ad77618d0a7514d129f41666a956a52d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:07:49 +0100 Subject: [PATCH 212/233] Update copyright info for 2014. Happy New Year everyone! --- src/drivers/frsky_telemetry/frsky_data.c | 4 ++-- src/drivers/frsky_telemetry/frsky_data.h | 4 ++-- src/drivers/frsky_telemetry/frsky_telemetry.c | 4 ++-- src/drivers/frsky_telemetry/module.mk | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 3a0c4b4b3b..9a7b6beeff 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index cea93ef2b9..a7d9eee533 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 5f3837b6ed..c4e29fe518 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 808966691a..1632c74f75 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 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 From 8fd909f51911b9ede93b19188ed360231f75de1c Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:08:44 +0100 Subject: [PATCH 213/233] Directly write to the voltage field for better precision. --- src/drivers/frsky_telemetry/frsky_data.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 9a7b6beeff..edbdbf366e 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -191,9 +191,7 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); - float voltage = battery.voltage_v * 11.0f / 21.0f; - frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage); - frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f); + frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); frsky_send_data(uart, FRSKY_ID_RPM, 0); From 1e7e65717a4522de59957d20be2b06ccc7b5a71d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:11:52 +0100 Subject: [PATCH 214/233] Only send data packets we really support. --- src/drivers/frsky_telemetry/frsky_data.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index edbdbf366e..9e6d57fac1 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -186,14 +186,9 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); - frsky_send_data(uart, FRSKY_ID_TEMP2, 0); - - frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ - frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); - - frsky_send_data(uart, FRSKY_ID_RPM, 0); + frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); frsky_send_startstop(uart); } From 1a21dcd34d218c524cede742dc5894d50e74b574 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 2 Jan 2014 08:28:33 +0100 Subject: [PATCH 215/233] ESC calib: low PWM value was not set --- src/systemcmds/esc_calib/esc_calib.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index b237b31be3..ad19966949 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -148,6 +148,7 @@ esc_calib_main(int argc, char *argv[]) case 'l': /* Read in custom low value */ + pwm_low = strtoul(optarg, &ep, 0); if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) usage("low PWM invalid"); break; From 85651218e28968161890ddcae1c670f08774c285 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 10:22:00 +0100 Subject: [PATCH 216/233] FMU-inspired PPM config --- src/drivers/boards/px4io-v1/board_config.h | 2 +- src/drivers/boards/px4io-v2/board_config.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 48aadbd768..d136a3dd86 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -92,4 +92,4 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN GPIO_TIM1_CH1IN +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 4d41d0d07e..3de6ddbd39 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -122,7 +122,7 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN GPIO_TIM1_CH1IN +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) /* LED definitions ******************************************************************/ /* PX4 has two LEDs that we will encode as: */ From 445b9b2339fe701a4234765bf49022c7d812d922 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 10:45:00 +0100 Subject: [PATCH 217/233] Final pin config for F1 PPM decoding, tested to be operational, pending in-application testing --- src/drivers/boards/px4io-v1/board_config.h | 2 +- src/drivers/boards/px4io-v2/board_config.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index d136a3dd86..a5fbfc7fd6 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -92,4 +92,4 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 3de6ddbd39..b6e298411b 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -122,7 +122,7 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) /* LED definitions ******************************************************************/ /* PX4 has two LEDs that we will encode as: */ From c4c652e9c629a11c5b113d4d26773fb54ff62af5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 13:18:05 +0100 Subject: [PATCH 218/233] tecs: change pitch on climbout #559 (ported from ardupilot) --- src/lib/external_lgpl/tecs/tecs.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a733ef1d2a..1d5c85699a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -404,10 +404,18 @@ void TECS::_update_pitch(void) // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence + // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle + // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the + // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + if (_climbOutDem) + { + temp += _PITCHminf * gainInv; + } _integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); + // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; From 6d08e9f6612ef0576e0064764e47817695e02c7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:09:33 +0100 Subject: [PATCH 219/233] HOTFIX: Avoid running out of range on the RC config params --- src/drivers/drv_rc_input.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 03f1dfbe58..6b87141e9d 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -60,7 +60,7 @@ /** * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 20 +#define RC_INPUT_MAX_CHANNELS 18 /** * Input signal type, value is a control position from zero to 100 From b2ef7f506cd433fb30332d7ab3e27134334f362f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:09:33 +0100 Subject: [PATCH 220/233] HOTFIX: Avoid running out of range on the RC config params --- src/drivers/drv_rc_input.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 03f1dfbe58..6b87141e9d 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -60,7 +60,7 @@ /** * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 20 +#define RC_INPUT_MAX_CHANNELS 18 /** * Input signal type, value is a control position from zero to 100 From 07fa4e3ec8474041f8c373c2b47f002bfdbb4788 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:26:57 +0100 Subject: [PATCH 221/233] Removed bogus 50 MHz setting, only relevant for outputs --- src/drivers/boards/px4io-v1/board_config.h | 4 ++-- src/drivers/boards/px4io-v2/board_config.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index a5fbfc7fd6..c3f39addfe 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -92,4 +92,4 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index b6e298411b..47ec4afe35 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -122,7 +122,7 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9) /* LED definitions ******************************************************************/ /* PX4 has two LEDs that we will encode as: */ From c11e36ad3deb110b18b43e09ffa4f240e36579df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:42:01 +0100 Subject: [PATCH 222/233] Board config sweep / cleanup. No further functionality-relevant points found --- src/drivers/boards/px4fmu-v1/board_config.h | 6 ++--- src/drivers/boards/px4fmu-v2/board_config.h | 26 ++++++++++----------- src/drivers/boards/px4io-v2/board_config.h | 2 +- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 27621211a1..6f71662846 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -34,7 +34,7 @@ /** * @file board_config.h * - * PX4FMU internal definitions + * PX4FMUv1 internal definitions */ #pragma once @@ -180,7 +180,7 @@ __BEGIN_DECLS #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) /**************************************************************************************************** * Public Types diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 7ab63953f8..a19ed9d247 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -34,7 +34,7 @@ /** * @file board_config.h * - * PX4FMU internal definitions + * PX4FMUv2 internal definitions */ #pragma once @@ -82,21 +82,21 @@ __BEGIN_DECLS #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* Data ready pins off */ -#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) -#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) -#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) +#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) +#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) +#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI1 off */ -#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) -#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6) -#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7) +#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) +#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6) +#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7) /* SPI1 chip selects off */ -#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) @@ -177,7 +177,7 @@ __BEGIN_DECLS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) */ -#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ #define HRT_TIMER 8 /* use timer8 for the HRT */ diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 47ec4afe35..8da5552110 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4iov2_internal.h + * @file board_config.h * * PX4IOV2 internal definitions */ From 4508972121196d8892520e56e6b374a59281e50a Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 22:34:08 +0100 Subject: [PATCH 223/233] Further data format and code style fixes. --- src/drivers/frsky_telemetry/frsky_data.c | 90 +++++++++---------- src/drivers/frsky_telemetry/frsky_telemetry.c | 19 ++-- 2 files changed, 53 insertions(+), 56 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 9e6d57fac1..63b2d2d29f 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,8 +53,7 @@ #include #include - -// FrSky sensor hub data IDs +/* FrSky sensor hub data IDs */ #define FRSKY_ID_GPS_ALT_BP 0x01 #define FRSKY_ID_TEMP1 0x02 #define FRSKY_ID_RPM 0x03 @@ -87,18 +86,13 @@ #define FRSKY_ID_VOLTS_BP 0x3A #define FRSKY_ID_VOLTS_AP 0x3B - #define frac(f) (f - (int)f) -float frsky_format_gps(float dec); - - static int battery_sub = -1; static int sensor_sub = -1; static int global_position_sub = -1; static int vehicle_status_sub = -1; - /** * Initializes the uORB subscriptions. */ @@ -113,8 +107,7 @@ void frsky_init() /** * Sends a 0x5E start/stop byte. */ -static void -frsky_send_startstop(int uart) +static void frsky_send_startstop(int uart) { static const uint8_t c = 0x5E; write(uart, &c, sizeof(c)); @@ -123,14 +116,12 @@ frsky_send_startstop(int uart) /** * Sends one byte, performing byte-stuffing if necessary. */ -static void -frsky_send_byte(int uart, uint8_t value) +static void frsky_send_byte(int uart, uint8_t value) { - const uint8_t x5E[] = {0x5D, 0x3E}; - const uint8_t x5D[] = {0x5D, 0x3D}; + const uint8_t x5E[] = { 0x5D, 0x3E }; + const uint8_t x5D[] = { 0x5D, 0x3D }; - switch (value) - { + switch (value) { case 0x5E: write(uart, x5E, sizeof(x5E)); break; @@ -148,10 +139,9 @@ frsky_send_byte(int uart, uint8_t value) /** * Sends one data id/value pair. */ -static void -frsky_send_data(int uart, uint8_t id, int16_t data) +static void frsky_send_data(int uart, uint8_t id, int16_t data) { - // Cast data to unsigned, because signed shift might behave incorrectly + /* Cast data to unsigned, because signed shift might behave incorrectly */ uint16_t udata = data; frsky_send_startstop(uart); @@ -163,7 +153,7 @@ frsky_send_data(int uart, uint8_t id, int16_t data) /** * Sends frame 1 (every 200ms): - * acceleration values, altitude (vario), temperatures, current & voltages, RPM + * acceleration values, barometer altitude, temperature, battery voltage & current */ void frsky_send_frame1(int uart) { @@ -178,17 +168,25 @@ void frsky_send_frame1(int uart) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* send formatted frame */ - frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f); + frsky_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter) * 100.0f)); - frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); + frsky_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius)); - frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); - frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); + frsky_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + frsky_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); frsky_send_startstop(uart); } @@ -196,47 +194,48 @@ void frsky_send_frame1(int uart) /** * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. */ -float frsky_format_gps(float dec) +static float frsky_format_gps(float dec) { - float dms_deg = (int)dec; - float dec_deg = dec - dms_deg; - float dms_min = (int)(dec_deg * 60); - float dec_min = (dec_deg * 60) - dms_min; - float dms_sec = dec_min * 60; + float dms_deg = (int) dec; + float dec_deg = dec - dms_deg; + float dms_min = (int) (dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; - return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); } /** * Sends frame 2 (every 1000ms): - * course, latitude, longitude, speed, altitude (GPS), fuel level + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level */ void frsky_send_frame2(int uart) { - /* get a local copy of the global position data */ + /* get a local copy of the global position data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - /* get a local copy of the vehicle status data */ + /* get a local copy of the vehicle status data */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); /* send formatted frame */ - float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; - if (global_pos.valid) - { + int sec = 0; + if (global_pos.valid) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); - lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f; + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) + * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; } @@ -258,7 +257,8 @@ void frsky_send_frame2(int uart) frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); - frsky_send_data(uart, FRSKY_ID_FUEL, vehicle_status.battery_remaining); + frsky_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); @@ -267,11 +267,11 @@ void frsky_send_frame2(int uart) /** * Sends frame 3 (every 5000ms): - * date, time + * GPS date & time */ void frsky_send_frame3(int uart) { - /* get a local copy of the battery data */ + /* get a local copy of the battery data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index c4e29fe518..7b08ca69e9 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -72,8 +72,7 @@ __EXPORT int frsky_telemetry_main(int argc, char *argv[]); /** * Opens the UART device and sets all required serial parameters. */ -static int -frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) { /* Open UART */ const int uart = open(uart_name, O_WRONLY | O_NOCTTY); @@ -118,20 +117,19 @@ frsky_open_uart(const char *uart_name, struct termios *uart_config_original) /** * Print command usage information */ -static void -usage() +static void usage() { - fprintf(stderr, "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + fprintf(stderr, + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); exit(1); } /** * The daemon thread. */ -static int -frsky_telemetry_thread_main(int argc, char *argv[]) +static int frsky_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ char *device_name = "/dev/ttyS1"; /* USART2 */ @@ -207,8 +205,7 @@ frsky_telemetry_thread_main(int argc, char *argv[]) * The main command function. * Processes command line arguments and starts the daemon. */ -int -frsky_telemetry_main(int argc, char *argv[]) +int frsky_telemetry_main(int argc, char *argv[]) { if (argc < 2) { warnx("missing command"); From 93a0c2de56e19cb01477a4c99aae4c33707e144b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jan 2014 13:51:20 +0100 Subject: [PATCH 224/233] Trimming down number of apps to save flash space --- makefiles/config_px4fmu-v1_backside.mk | 15 --------------- makefiles/config_px4fmu-v1_default.mk | 10 ++++------ 2 files changed, 4 insertions(+), 21 deletions(-) diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index 5ecf93a3a1..c86beaccae 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -25,14 +25,10 @@ MODULES += drivers/bma180 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled -MODULES += drivers/mkblctrl MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed @@ -71,22 +67,11 @@ MODULES += modules/gpio_led # # Estimation modules (EKF/ SO3 / other filters) # -#MODULES += modules/attitude_estimator_ekf MODULES += modules/att_pos_estimator_ekf -#MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator -MODULES += modules/attitude_estimator_so3 # # Vehicle Control # -#MODULES += modules/segway # XXX Needs GCC 4.7 fix -#MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control -#MODULES += modules/multirotor_att_control -#MODULES += modules/multirotor_pos_control -#MODULES += examples/flow_position_control -#MODULES += examples/flow_speed_control MODULES += modules/fixedwing_backside # diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 25c22f1fdf..e76967702b 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -21,7 +21,7 @@ MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu-v1 MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 -MODULES += drivers/bma180 +#MODULES += drivers/bma180 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 @@ -33,7 +33,6 @@ MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl -MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed @@ -75,18 +74,17 @@ MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator +#MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control -MODULES += examples/flow_position_control -MODULES += examples/flow_speed_control +#MODULES += examples/flow_position_control +#MODULES += examples/flow_speed_control # # Logging From ba4f881f807e75edc75d2ad2084dd1ebbbec3802 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:49:21 +0100 Subject: [PATCH 225/233] Do not require suffixed constants --- makefiles/toolchain_gnu-arm-eabi.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 9fd2dd516d..bb729e1032 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -138,8 +138,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs # C++-specific warnings # From 76477b205775ddcbce6147d93985ddcdb10a3d52 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jan 2014 09:25:39 +0100 Subject: [PATCH 226/233] Added support for Octo Cox --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f122921c56..66cb3f2375 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -213,7 +213,9 @@ then # 7000 .. 7999 Hexa + # 8000 .. 8999 Octo X # 9000 .. 9999 Octo + - # 10000 .. 19999 Wide arm / H frame + # 10000 .. 10999 Wide arm / H frame + # 11000 .. 11999 Hexa Cox + # 12000 .. 12999 Octo Cox if param compare SYS_AUTOSTART 4008 8 then @@ -277,6 +279,13 @@ then sh /etc/init.d/rc.octo set MODE custom fi + + if param compare SYS_AUTOSTART 12001 + then + set MIXER /etc/mixers/FMU_octo_cox.mix + sh /etc/init.d/rc.octo + set MODE custom + fi if param compare SYS_AUTOSTART 10015 15 then From 1a13e66aab3cd88b5447448b577fc44165ab01bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 15:59:18 +0800 Subject: [PATCH 227/233] px4iofirmware: make forceupdate more reliable this schedules a reboot rather than rebooting immediately, which means the FMU gets an ACK for its reboot operation, preventing it from timing out waiting for the ACK. That makes the timing of the reboot more consistent, which makes it more reliable for forceupdate --- src/modules/px4iofirmware/px4io.c | 21 +++++++++++++++++++++ src/modules/px4iofirmware/px4io.h | 4 ++++ src/modules/px4iofirmware/registers.c | 13 ++++--------- 3 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705f..0b8c4a6a8b 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -125,6 +125,25 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} static void calculate_fw_crc(void) @@ -249,6 +268,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + check_reboot(); + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea04a663b..a0daa97ea0 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -220,3 +220,7 @@ extern volatile uint8_t debug_level; /** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0358725db3..ad44730736 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -518,16 +518,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) break; - - // note that we don't set BL_WAIT_MAGIC in - // BKP_DR1 as that is not necessary given the - // timing of the forceupdate command. The - // bootloader on px4io waits for enough time - // anyway, and this method works with older - // bootloader versions (tested with both - // revision 3 and revision 4). - up_systemreset(); + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: From c4fc730acad12b74f51d9ba7d3ff267e3e1a1ab3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:13:51 +0800 Subject: [PATCH 228/233] FMUv2: make all UARTs use 512 byte buffer, 2048 for CDCACM output this is important when using UARTs for things like secondary GPS modules, which may produce large enough transfers that 128 bytes is not enough. The 2048 buffer for CDCACM transmit makes mavlink log and parameter transfer faster --- nuttx-configs/px4fmu-v2/nsh/defconfig | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 110bcb3637..e20411bca3 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=128 -CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_RXBUFSIZE=512 +CONFIG_UART4_TXBUFSIZE=512 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From d4d2571161530848f2a4ac153f2529ab50ec4fcc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:25:09 +0800 Subject: [PATCH 229/233] FMUv2: enable RXDMA on 2nd GPS port and debug console This should reduce the chance of lost data on these ports due to high interrupt latency --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e20411bca3..8a282693f4 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -295,7 +295,7 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -# CONFIG_USART1_RXDMA is not set +CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set @@ -304,7 +304,7 @@ CONFIG_USART3_RXDMA=y CONFIG_UART4_RXDMA=y # CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set From 1f564a95ee89278b3e4eea6c2d4ded378b71542c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:37:40 +0800 Subject: [PATCH 230/233] meas_airspeed: avoid trivial dependency on math lib including the math lib adds a huge amount to flash usage --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 3cd6d67203..a95c4576b0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -198,7 +198,9 @@ MEASAirspeed::collect() // uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + if (diff_press_pa < 0.0f) + diff_press_pa = 0.0f; struct differential_pressure_s report; From 3be1a5182db7bd3802b77e7c03fc14f00ca218c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 22:39:51 +1100 Subject: [PATCH 231/233] FMUv1: use larger CDCACM buffer size for faster log transfer on FMUv1 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e43b9c18e8..e60120b492 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -566,7 +566,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" From d6088efd34e5482d302e3a253fdd3a170d88490a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 10:32:15 +1100 Subject: [PATCH 232/233] ms5611: report P and T in ms5611 info --- src/drivers/ms5611/ms5611.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 87788824a9..6326cf7fcb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -124,6 +124,8 @@ protected: int32_t _TEMP; int64_t _OFF; int64_t _SENS; + float _P; + float _T; /* altitude conversion calibration */ unsigned _msl_pressure; /* in kPa */ @@ -623,6 +625,8 @@ MS5611::collect() /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; /* generate a new report */ report.temperature = _TEMP / 100.0f; @@ -695,6 +699,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", _P); + printf("T: %.3f\n", _T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); From 94b539dfddc5a2e293f51058ee5bf0d6ffc78406 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 18 Sep 2013 10:30:09 +1000 Subject: [PATCH 233/233] px4io: enable power on Spektrum connector on init --- src/modules/px4iofirmware/dsm.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4d306d6d00..60eda23192 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -203,6 +203,12 @@ dsm_guess_format(bool reset) int dsm_init(const char *device) { + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + if (dsm_fd < 0) dsm_fd = open(device, O_RDONLY | O_NONBLOCK);