From ab5ec0da0b4afcb3f68eaf70efebe691513f74ce Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:53:19 +0200 Subject: [PATCH 01/40] Changed the default PID gains for the F330 --- ROMFS/px4fmu_common/init.d/10_io_f330 | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index b2fc0c96f1..50842764a6 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -16,19 +16,19 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.005 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.1 + 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 4.5 + param set MC_ATT_P 7.0 param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.3 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.1 + 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 save /fs/microsd/params fi From ca96140b217971d527e2306922a87a1592e6f90d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:53:46 +0200 Subject: [PATCH 02/40] Allow the tone alarms to be interrupted --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index b06920a765..ad21f7143f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -879,14 +879,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _tune = nullptr; _next = nullptr; } else { - /* don't interrupt alarms unless they are repeated */ - if (_tune != nullptr && !_repeat) { - /* abort and let the current tune finish */ - result = -EBUSY; - } else if (_repeat && _default_tune_number == arg) { - /* requested repeating tune already playing */ - } else { - // play the selected tune + /* always interrupt alarms, unless they are repeating and already playing */ + if (!(_repeat && _default_tune_number == arg)) { + /* play the selected tune */ _default_tune_number = arg; start_tune(_default_tunes[arg - 1]); } From 6c3da5aeddf929f5a4f19f6bd1b75c911c2a414c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:55:33 +0200 Subject: [PATCH 03/40] Reset yaw position when disarmed in multirotor controller --- .../multirotor_att_control_main.c | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c057ef3647..2d16d49219 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -144,10 +144,6 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; - bool control_yaw_position = true; bool reset_yaw_sp = true; @@ -232,12 +228,6 @@ mc_thread_main(int argc, char *argv[]) } else if (control_mode.flag_control_manual_enabled) { /* direct manual input */ if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - /* initialize to current yaw if switching to manual or att control */ - if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { - att_sp.yaw_body = att.yaw; - } static bool rc_loss_first_time = true; @@ -283,12 +273,12 @@ mc_thread_main(int argc, char *argv[]) /* control yaw in all manual / assisted modes */ /* set yaw if arming or switching to attitude stabilized mode */ - if (!flag_control_attitude_enabled) { + if (!control_mode.flag_armed) { reset_yaw_sp = true; } /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { // TODO use landed status instead of throttle rates_sp.yaw = manual.yaw; control_yaw_position = false; reset_yaw_sp = true; @@ -377,10 +367,6 @@ mc_thread_main(int argc, char *argv[]) actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; - flag_control_manual_enabled = control_mode.flag_control_manual_enabled; - perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ } /* end of poll return value check */ From 5f1004117f8086c4bba5b4031f3aebd73411682c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:57:17 +0200 Subject: [PATCH 04/40] Restore proper feedback (mavlink and tone) for calibration commands, etc --- .../commander/accelerometer_calibration.cpp | 6 +- .../commander/accelerometer_calibration.h | 2 +- .../commander/airspeed_calibration.cpp | 18 +- src/modules/commander/airspeed_calibration.h | 4 +- src/modules/commander/baro_calibration.cpp | 10 +- src/modules/commander/baro_calibration.h | 4 +- src/modules/commander/commander.cpp | 198 ++++++++++-------- src/modules/commander/gyro_calibration.cpp | 31 +-- src/modules/commander/gyro_calibration.h | 4 +- src/modules/commander/mag_calibration.cpp | 17 +- src/modules/commander/mag_calibration.h | 4 +- src/modules/commander/rc_calibration.cpp | 13 +- src/modules/commander/rc_calibration.h | 4 +- 13 files changed, 179 insertions(+), 136 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ddd2e23d95..c2b2e92581 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -133,7 +133,7 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int mavlink_fd) { +int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); @@ -176,11 +176,11 @@ void do_accel_calibration(int mavlink_fd) { } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_positive(); + return OK; } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_negative(); + return ERROR; } /* exit accel calibration mode */ diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 3275d94612..1cf9c09775 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -45,6 +45,6 @@ #include -void do_accel_calibration(int mavlink_fd); +int do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index df08292e32..e414e5f707 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,7 +49,13 @@ #include #include -void do_airspeed_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); @@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; + return ERROR; } } @@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + return ERROR; } /* auto-save to EEPROM */ @@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } //char buf[50]; @@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_positive(); + return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + return ERROR; } - - close(diff_pres_sub); } diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 92f5651ec1..71c701fc2e 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -41,6 +41,6 @@ #include -void do_airspeed_calibration(int mavlink_fd); +int do_airspeed_calibration(int mavlink_fd); -#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file +#endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index d7515b3d9b..3123c40876 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -47,8 +47,14 @@ #include #include -void do_baro_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_baro_calibration(int mavlink_fd) { // TODO implement this - return; + return ERROR; } diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index ac0f4be368..bc42bc6cfc 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -41,6 +41,6 @@ #include -void do_baro_calibration(int mavlink_fd); +int do_baro_calibration(int mavlink_fd); -#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* BARO_CALIBRATION_H_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17db0f9c88..66b9272de6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -502,7 +502,13 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + /* try again later */ + usleep(20000); + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); + } } /* Main state machine */ @@ -1628,6 +1634,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v return res; } +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(); + break; + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + default: + break; + } +} + void *commander_low_prio_loop(void *arg) { /* Set thread name */ @@ -1668,125 +1701,110 @@ void *commander_low_prio_loop(void *arg) cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) continue; - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle low-priority commands here */ switch (cmd.command) { case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { + if (is_safe(&status, &safety, &armed)) { - if (((int)(cmd.param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd.param1)) == 3) { - /* reboot to bootloader */ - systemreset(true); - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot to bootloader */ + systemreset(true); } else { - result = VEHICLE_CMD_RESULT_DENIED; + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - /* try to go to INIT/PREFLIGHT arming state */ + int calib_ret = ERROR; - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - result = VEHICLE_CMD_RESULT_DENIED; - break; - } - - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - do_gyro_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - do_mag_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - do_accel_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - do_airspeed_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } - - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + /* try to go to INIT/PREFLIGHT arming state */ + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } - case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } - } + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); - break; + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); } - default: + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); + + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - } else { - tune_negative(); + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } } + break; + } + + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; } /* send any requested ACKs */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index f1afb0107e..9cd2f3a19c 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -50,8 +50,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_gyro_calibration(int mavlink_fd) +int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); @@ -98,7 +103,7 @@ void do_gyro_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; + return ERROR; } } @@ -137,18 +142,17 @@ void do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); - // XXX negative tune - return; + return ERROR; } mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); + tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - return; + return ERROR; } @@ -180,8 +184,7 @@ void do_gyro_calibration(int mavlink_fd) && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); - return; + return OK; } /* wait blocking for new data */ @@ -221,7 +224,7 @@ void do_gyro_calibration(int mavlink_fd) // operating near the 1e6/1e8 max sane resolution of float. gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); +// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); // } else if (poll_ret == 0) { // /* any poll failure for 1s is a reason to abort */ @@ -232,8 +235,8 @@ void do_gyro_calibration(int mavlink_fd) float gyro_scale = baseline_integral / gyro_integral; float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; - warnx("gyro scale: yaw (z): %6.4f", gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { @@ -272,12 +275,10 @@ void do_gyro_calibration(int mavlink_fd) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); /* third beep by cal end routine */ - + return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + return ERROR; } - - close(sub_sensor_combined); } diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h index cd262507db..59c32a15e9 100644 --- a/src/modules/commander/gyro_calibration.h +++ b/src/modules/commander/gyro_calibration.h @@ -41,6 +41,6 @@ #include -void do_gyro_calibration(int mavlink_fd); +int do_gyro_calibration(int mavlink_fd); -#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9a25103f8c..9263c6a159 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -53,8 +53,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_mag_calibration(int mavlink_fd) +int do_mag_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); @@ -113,7 +118,7 @@ void do_mag_calibration(int mavlink_fd) warnx("mag cal failed: out of memory"); mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); warnx("x:%p y:%p z:%p\n", x, y, z); - return; + return ERROR; } while (hrt_absolute_time() < calibration_deadline && @@ -252,6 +257,7 @@ void do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", @@ -269,12 +275,11 @@ void do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_positive(); + return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + return ERROR; } - - close(sub_mag); -} \ No newline at end of file +} diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index fd2085f14a..a101cd7b1f 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -41,6 +41,6 @@ #include -void do_mag_calibration(int mavlink_fd); +int do_mag_calibration(int mavlink_fd); -#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file +#endif /* MAG_CALIBRATION_H_ */ diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0de411713b..fe87a3323b 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -46,8 +46,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_rc_calibration(int mavlink_fd) +int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); @@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + return ERROR; } - tune_positive(); - mavlink_log_info(mavlink_fd, "trim calibration done"); -} \ No newline at end of file + return OK; +} diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 6505c73647..9aa6faafa8 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include -void do_rc_calibration(int mavlink_fd); +int do_rc_calibration(int mavlink_fd); -#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file +#endif /* RC_CALIBRATION_H_ */ From b5bb20995be8c0f55bed4f2f2bd6cee9efdcf03e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 17:31:59 +0200 Subject: [PATCH 05/40] multirotor_att_control: yaw setpoint reset fix --- .../multirotor_att_control_main.c | 101 +++++++++--------- 1 file changed, 52 insertions(+), 49 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c057ef3647..2d46bf4387 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -81,6 +81,8 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; +static const float min_takeoff_throttle = 0.3f; +static const float yaw_deadzone = 0.01f; static int mc_thread_main(int argc, char *argv[]) @@ -147,14 +149,14 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool control_yaw_position = true; bool reset_yaw_sp = true; + bool failsafe_first_time = true; /* prepare the handle for the failsafe throttle */ param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - + param_get(failsafe_throttle_handle, &failsafe_throttle); while (!thread_should_exit) { @@ -176,7 +178,7 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - // XXX no params here yet + param_get(failsafe_throttle_handle, &failsafe_throttle); } /* only run controller if attitude changed */ @@ -208,6 +210,9 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + /* set flag to safe value */ + control_yaw_position = true; + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ @@ -225,47 +230,40 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } else if (control_mode.flag_control_manual_enabled) { - /* direct manual input */ + /* manual input */ if (control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ - /* initialize to current yaw if switching to manual or att control */ - if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { + failsafe_first_time = false; + if (!control_mode.flag_control_velocity_enabled) { - /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ + /* don't reset attitude setpoint in position control mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; + } - if (!control_mode.flag_control_climb_rate_enabled) { - /* Don't touch throttle in modes with altitude hold, it's handled by position controller. - * - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.thrust = failsafe_throttle; + if (!control_mode.flag_control_climb_rate_enabled) { + /* don't touch throttle in modes with altitude hold, it's handled by position controller. + * + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + att_sp.thrust = failsafe_throttle; - } else { - att_sp.thrust = 0.0f; - } + } else { + att_sp.thrust = 0.0f; } } @@ -273,46 +271,48 @@ mc_thread_main(int argc, char *argv[]) * since if the pilot regains RC control, he will be lost regarding * the current orientation. */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; + if (failsafe_first_time) { + reset_yaw_sp = true; + } } else { - rc_loss_first_time = true; + failsafe_first_time = true; /* control yaw in all manual / assisted modes */ /* set yaw if arming or switching to attitude stabilized mode */ - if (!flag_control_attitude_enabled) { + if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) { reset_yaw_sp = true; } /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle - rates_sp.yaw = manual.yaw; + // TODO review yaw restpoint reset + if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) { + /* control yaw rate */ control_yaw_position = false; - reset_yaw_sp = true; + rates_sp.yaw = manual.yaw; + reset_yaw_sp = true; // has no effect on control, just for beautiful log } else { - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } control_yaw_position = true; } if (!control_mode.flag_control_velocity_enabled) { - /* don't update attitude setpoint in position control mode */ + /* update attitude setpoint if not in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; if (!control_mode.flag_control_climb_rate_enabled) { - /* don't set throttle in altitude hold modes */ + /* pass throttle directly if not in altitude control mode */ att_sp.thrust = manual.throttle; } } - att_sp.timestamp = hrt_absolute_time(); + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; } if (motor_test_mode) { @@ -321,10 +321,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); } - /* STEP 2: publish the controller output */ + att_sp.timestamp = hrt_absolute_time(); + + /* publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { @@ -367,6 +368,7 @@ mc_thread_main(int argc, char *argv[]) rates[1] = att.pitchspeed; rates[2] = att.yawspeed; multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; @@ -374,6 +376,7 @@ mc_thread_main(int argc, char *argv[]) actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); From bb91484b2648800923a135be28924119a1382ba6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 17:34:59 +0200 Subject: [PATCH 06/40] Default flight mode switches parameters changed. --- src/modules/sensors/sensor_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2bd869263c..8c2beb4561 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,8 +167,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); From 41fac46ff08787d9b2e4d902045d65c205389abd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 18:05:30 +0200 Subject: [PATCH 07/40] mavlink VFR_HUD message fixed, minor fixes and cleanup --- src/modules/commander/commander.cpp | 4 ++-- src/modules/mavlink/orb_listener.c | 10 +++++----- .../position_estimator_inav_main.c | 2 +- .../uORB/topics/vehicle_global_position.h | 18 +++++++++--------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04e6dd2cb2..4580c57bc5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -825,9 +825,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = local_position.landed; status_changed = true; if (status.condition_landed) { - mavlink_log_info(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); } else { - mavlink_log_info(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d3..97cf571e52 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -637,12 +637,12 @@ l_airspeed(const struct listener *l) 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); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + uint16_t throttle = actuators_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, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } static void * 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 3466841c4c..d35755b4f3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -610,7 +610,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index f036c72237..822c323cfc 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,17 +62,17 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees LOGME */ - float alt; /**< Altitude in meters LOGME */ - float relative_alt; /**< Altitude above home position in meters, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + float alt; /**< Altitude in meters */ + float relative_alt; /**< Altitude above home position in meters, */ + float vx; /**< Ground X velocity, m/s in NED */ + float vy; /**< Ground Y velocity, m/s in NED */ + float vz; /**< Ground Z velocity, m/s in NED */ + float hdg; /**< Compass heading in radians -PI..+PI. */ }; From 330908225e5fcb1731df20e740dbfe403a7b30b9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 18:23:42 +0200 Subject: [PATCH 08/40] sdlog2: free buffer on exit --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7f8648d95d..2b21bb5a5a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1182,6 +1182,8 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); + free(lb.data); + warnx("exiting."); thread_running = false; From ca9aabf48bbf4ab050a13f12e8152490e2b1dbde Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:29:06 -0700 Subject: [PATCH 09/40] add placeholder autoconfig file for X550 --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 107 +++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 new file mode 100644 index 0000000000..41593fa6c0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi From 64f402ce839f619572bb45a7b1a3126a2cd96d88 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:29:06 -0700 Subject: [PATCH 10/40] add placeholder autoconfig file for X550 --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 107 +++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 new file mode 100644 index 0000000000..41593fa6c0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi From 201bda457941fa55961eea4f13ec9fc9c24c0c61 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:36:10 -0700 Subject: [PATCH 11/40] start position estimator and position control --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 index 41593fa6c0..a7ffffaeec 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -82,6 +82,10 @@ gps start # Start the attitude estimator # attitude_estimator_ekf start +# +# Start the position estimator +# +position_estimator_inav start echo "[init] starting PWM output" fmu mode_pwm @@ -93,6 +97,11 @@ pwm -u 400 -m 0xff # multirotor_att_control start +# +# Start position control +# +multirotor_pos_control start + # # Start logging # From f70a4b3b7045a24baf70e642e9a354a13e5fa7d1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 22 Aug 2013 23:47:55 -0700 Subject: [PATCH 12/40] Add support for adding extra files to the ROMFS from the config. If there is an IO firmware image already built when we build the corresponding FMU ROMFS, copy it into the ROMFS. Note - due to there being no fixed build ordering, to be certain that you have the most current IO firmware, you must build the IO firmware explicitly first. --- Makefile | 26 +++++++++++++------------- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 4 +++- makefiles/firmware.mk | 22 +++++++++++++++++++--- makefiles/setup.mk | 1 + 5 files changed, 37 insertions(+), 17 deletions(-) diff --git a/Makefile b/Makefile index f7cab05c86..c34945ecab 100644 --- a/Makefile +++ b/Makefile @@ -114,8 +114,8 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% - $(Q) mkdir -p $(work_dir) - $(Q) make -r -C $(work_dir) \ + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -147,12 +147,12 @@ $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export - $(Q) mkdir -p $(dir $@) + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -168,11 +168,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else @@ -191,7 +191,7 @@ $(NUTTX_SRC): # Testing targets # testbuild: - $(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8) + $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) # # Cleanup targets. 'clean' should remove all built products and force @@ -206,8 +206,8 @@ clean: .PHONY: distclean distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete) + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) # # Print some help text @@ -229,9 +229,9 @@ help: @$(ECHO) " A limited set of configs can be built with CONFIGS=" @$(ECHO) "" @for config in $(CONFIGS); do \ - echo " $$config"; \ - echo " Build just the $$config firmware configuration."; \ - echo ""; \ + $(ECHO) " $$config"; \ + $(ECHO) " Build just the $$config firmware configuration."; \ + $(ECHO) ""; \ done @$(ECHO) " clean" @$(ECHO) " Remove all firmware build pieces." diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 8f2ade8dc8..0e4617f1db 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -6,6 +6,7 @@ # 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 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 101b497125..61df64a78c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -3,9 +3,11 @@ # # -# Use the configuration's ROMFS. +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin # # Board support modules diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 8a027a0a81..b3e50501c7 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -322,7 +322,7 @@ endif # a root from several templates. That would be a nice feature. # -# Add dependencies on anything in the ROMFS root +# Add dependencies on anything in the ROMFS root directory ROMFS_FILES += $(wildcard \ $(ROMFS_ROOT)/* \ $(ROMFS_ROOT)/*/* \ @@ -334,7 +334,14 @@ ifeq ($(ROMFS_FILES),) $(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files) endif ROMFS_DEPS += $(ROMFS_FILES) + +# Extra files that may be copied into the ROMFS /extras directory +# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional +ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES)) +ROMFS_DEPS += $(ROMFS_EXTRA_FILES) + ROMFS_IMG = romfs.img +ROMFS_SCRATCH = romfs_scratch ROMFS_CSRC = $(ROMFS_IMG:.img=.c) ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) LIBS += $(ROMFS_OBJ) @@ -345,9 +352,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) $(call BIN_TO_OBJ,$<,$@,romfs_img) # Generate the ROMFS image from the root -$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS) +$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) @$(ECHO) "ROMFS: $@" - $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol" + $(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol" + +# Construct the ROMFS scratch root from the canonical root +$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) + $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) +ifneq ($(ROMFS_EXTRA_FILES),) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras + $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras +endif EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 168e41a5cb..51f830b6ee 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -71,6 +71,7 @@ export RMDIR = rm -rf export GENROMFS = genromfs export TOUCH = touch export MKDIR = mkdir +export FIND = find export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python From 54711bbcfe52fe33c213b9e15a1da9ad978d3535 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 23 Aug 2013 00:23:32 -0700 Subject: [PATCH 13/40] In order to save people from themselves, force a given FMU version to depend on the corresponding _default IO version. This avoids the risk of building a new FMU ROMFS with an old IO firmware, at the cost of the sanity of anyone reading this. --- Makefile | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Makefile b/Makefile index c34945ecab..cbf0da6c97 100644 --- a/Makefile +++ b/Makefile @@ -121,6 +121,19 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: WORK_DIR=$(work_dir) \ $(FIRMWARE_GOAL) +# +# Make FMU firmwares depend on pre-packaged IO binaries. +# +# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency +# and forces the _default config in all cases. There has to be a better way to do this... +# +FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) +define FMU_DEP +$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 +endef +FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) +$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) + # # Build the NuttX export archives. # From d1fd1bbbf7c7fd6bc76137257d6ecb91e8b6f3d4 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 23 Aug 2013 13:27:16 -0700 Subject: [PATCH 14/40] Fix timestamp on rates_sp --- .../multirotor_att_control/multirotor_attitude_control.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 12d16f7c73..c78232f11c 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -247,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } rates_sp->thrust = att_sp->thrust; + //need to update the timestamp now that we've touched rates_sp + rates_sp->timestamp = hrt_absolute_time(); motor_skip_counter++; } From 5e9b508ea0ec799ab6f8723d114c999beffc347e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 23 Aug 2013 23:03:59 +0200 Subject: [PATCH 15/40] Indicate AUTO submodes in mavlink custom_mode. --- src/modules/commander/commander.cpp | 74 +++++++++++++----------- src/modules/commander/px4_custom_mode.h | 29 ++++++++-- src/modules/mavlink/mavlink.c | 24 ++++++-- src/modules/mavlink/mavlink_receiver.cpp | 5 +- 4 files changed, 87 insertions(+), 45 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4580c57bc5..8bde6b7a90 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -274,7 +274,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - uint32_t custom_mode = (uint32_t) cmd->param2; + union px4_custom_mode custom_mode; + custom_mode.data_float = cmd->param2; // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); @@ -307,19 +308,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } @@ -1544,43 +1545,46 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; - - } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } } + } else { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); } - break; default: diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 4d4859a5c5..b60a7e0b9c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,11 +8,30 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, +enum PX4_CUSTOM_MAIN_MODE { + PX4_CUSTOM_MAIN_MODE_MANUAL = 1, + PX4_CUSTOM_MAIN_MODE_SEATBELT, + PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_AUTO, +}; + +enum PX4_CUSTOM_SUB_MODE_AUTO { + PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION, + PX4_CUSTOM_SUB_MODE_AUTO_RTL, + PX4_CUSTOM_SUB_MODE_AUTO_LAND, +}; + +union px4_custom_mode { + struct { + uint16_t reserved; + uint8_t main_mode; + uint8_t sub_mode; + }; + uint32_t data; + float data_float; }; #endif /* PX4_CUSTOM_MODE_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 6d9ca1120e..93ec36d05f 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u /* main state */ *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; if (v_status.main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; } else if (v_status.main_state == MAIN_STATE_EASY) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } } + *mavlink_custom_mode = custom_mode.data; /** * Set mavlink state diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46d..86fa736562 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -67,6 +67,7 @@ #include #include #include +#include __BEGIN_DECLS @@ -188,9 +189,11 @@ handle_message(mavlink_message_t *msg) mavlink_set_mode_t new_mode; mavlink_msg_set_mode_decode(msg, &new_mode); + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; + vcmd.param2 = custom_mode.data_float; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; From 8579d0b7c9f77c84fa9afa87f7ab2f353443a242 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 24 Aug 2013 20:31:01 +0200 Subject: [PATCH 16/40] Allow disarm by RC in assisted modes if landed and in AUTO_READY state. --- src/modules/commander/commander.cpp | 55 ++++++++++++++--------------- 1 file changed, 26 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fbb..f5a9d4088d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1017,46 +1017,42 @@ int commander_thread_main(int argc, char *argv[]) /* arm/disarm by RC */ res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - stick_on_counter = 0; + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; } else { - stick_off_counter = 0; + stick_off_counter++; } + } else { + stick_off_counter = 0; } - /* check if left stick is in lower right position and we're in manual mode -> arm */ + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - status.main_state == MAIN_STATE_MANUAL) { - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - stick_on_counter = 0; - - } else { - stick_on_counter++; - } - - stick_off_counter = 0; + status.main_state == MAIN_STATE_MANUAL && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { - stick_on_counter = 0; + stick_on_counter++; } + + } else { + stick_on_counter = 0; } if (res == TRANSITION_CHANGED) { @@ -1702,7 +1698,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || - cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF) continue; /* only handle low-priority commands here */ From 41dfdfb1a4b974b5d32788852768513d0dac7a67 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 24 Aug 2013 20:32:46 +0200 Subject: [PATCH 17/40] Use common rc.multirotor script (now only in 01_fmu_quad_x). --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 33 +++------------- ROMFS/px4fmu_common/init.d/rc.multirotor | 49 ++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 28 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index c1f3187f93..8223b3ea5a 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -62,44 +62,21 @@ param set MAV_TYPE 2 # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start # -# Start GPS interface (depends on orb) +# Start common for all multirotors apps # -gps start - +sh /etc/init.d/rc.multirotor + # -# Start the attitude estimator +# Start PWM output # -attitude_estimator_ekf start - -echo "[init] starting PWM output" fmu mode_pwm mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix pwm -u 400 -m 0xff -# -# Start attitude control -# -multirotor_att_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - # Try to get an USB console nshterm /dev/ttyACM0 & # Exit, because /dev/ttyS0 is needed for MAVLink -exit +#exit diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor new file mode 100644 index 0000000000..81184f3632 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -0,0 +1,49 @@ +#!nsh +# +# Standard everything needed for multirotors except mixer, output and mavlink +# + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +# +# Start logging +# +if [ $BOARD == fmuv1 ] +then + sdlog2 start -r 50 -a -b 16 +else + sdlog2 start -r 200 -a -b 16 +fi From e119bbb0f1161c71b8c2dcbbbc150d40b356c4b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 16:33:14 +0200 Subject: [PATCH 18/40] A lot more on calibration and RC checks. Needs more testing, but no known issues --- ROMFS/px4fmu_common/init.d/rcS | 6 + .../commander/accelerometer_calibration.cpp | 18 ++- src/modules/commander/commander.cpp | 6 + src/modules/commander/mag_calibration.cpp | 10 +- src/modules/sensors/sensor_params.c | 10 +- src/modules/sensors/sensors.cpp | 37 +---- src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/rc_check.c | 148 ++++++++++++++++++ src/modules/systemlib/rc_check.h | 52 ++++++ .../preflight_check/preflight_check.c | 97 +----------- 10 files changed, 246 insertions(+), 141 deletions(-) create mode 100644 src/modules/systemlib/rc_check.c create mode 100644 src/modules/systemlib/rc_check.h diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b2780..30edf679ad 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,6 +182,12 @@ fi # Try to get an USB console nshterm /dev/ttyACM0 & +# Start any custom extensions that might be missing +if [ -f /fs/microsd/etc/rc.local ] +then + sh /fs/microsd/etc/rc.local +fi + # If none of the autostart scripts triggered, get a minimal setup if [ $MODE == autostart ] then diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c2b2e92581..82df7c37d6 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; @@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + unsigned done_count = 0; + while (true) { bool done = true; - char str[80]; + char str[60]; int str_ptr; str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; for (int i = 0; i < 6; i++) { if (!data_collected[i]) { str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; + } else { + done_count++; } } + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + if (done) break; mavlink_log_info(mavlink_fd, str); @@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float continue; } - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + // sprintf(str, + mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fbb..e3d314881c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; + bool rc_calibration_ok = (OK == rc_calibration_check()); + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); status_changed = true; + + /* Re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check()); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9263c6a159..42f0190c7e 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd) close(fd); + mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + /* calibrate offsets */ // uint64_t calibration_start = hrt_absolute_time(); @@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd) warnx("Setting Z mag scale failed!\n"); } + mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + /* auto-save to EEPROM */ int save_ret = param_save_default(); @@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; /* third beep by cal end routine */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index b45317dbef..fd2a6318eb 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ @@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ded39c4651..e857b1c2f7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -305,8 +305,6 @@ private: int board_rotation; int external_mag_rotation; - int rc_type; - int rc_map_roll; int rc_map_pitch; int rc_map_yaw; @@ -342,9 +340,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -566,8 +561,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -692,11 +685,6 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -738,26 +726,11 @@ Sensors::parameters_update() // warnx("Failed getting offboard control mode sw chan index"); // } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); - } - - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); - } - - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); - } - - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); - } - - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } - + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 94c744c039..843cda7225 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ systemlib.c \ airspeed.c \ system_params.c \ - mavlink_log.c + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 0000000000..9d47d8000d --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 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 rc_check.c + * + * RC calibration check + */ + +#include + +#include +#include + +#include +#include +#include +#include + +int rc_calibration_check(void) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + } +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 0000000000..e2238d1516 --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 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 rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(void); + +__END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e7d9ce85fb..4c19dcffb6 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include #include +#include __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); @@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - float param_min, param_max, param_trim, param_rev, param_dz; - - bool rc_ok = true; - char nbuf[20]; - - /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - for (int i = 0; i < 12; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < 500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_max > 2500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim < param_min) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim > param_max) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - - /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - count++; - } - - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - /* sanity checks pass, enable channel */ - if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); - rc_ok = false; - } - } + bool rc_ok = (OK == rc_calibration_check()); /* require RC ok to keep system_ok */ system_ok &= rc_ok; From 557d3f22de25a392995f2803363f32fdbc6ce843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 19:27:11 +0200 Subject: [PATCH 19/40] Startup scripts major cleanup --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 58 ++-- ROMFS/px4fmu_common/init.d/02_io_quad_x | 90 ++---- ROMFS/px4fmu_common/init.d/08_ardrone | 79 ++--- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 67 ++-- ROMFS/px4fmu_common/init.d/10_io_f330 | 90 ++---- ROMFS/px4fmu_common/init.d/15_io_tbs | 91 +----- ROMFS/px4fmu_common/init.d/30_io_camflyer | 64 +--- ROMFS/px4fmu_common/init.d/31_io_phantom | 93 +----- ROMFS/px4fmu_common/init.d/40_io_segway | 84 +---- .../{666_fmu_quad_x550 => 666_fmu_q_x550} | 84 +---- ROMFS/px4fmu_common/init.d/rc.io | 23 ++ ROMFS/px4fmu_common/init.d/rc.logging | 7 +- ROMFS/px4fmu_common/init.d/rc.multirotor | 10 - ROMFS/px4fmu_common/init.d/rcS | 301 +++++++++--------- 14 files changed, 338 insertions(+), 803 deletions(-) rename ROMFS/px4fmu_common/init.d/{666_fmu_quad_x550 => 666_fmu_q_x550} (50%) create mode 100644 ROMFS/px4fmu_common/init.d/rc.io diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index 8223b3ea5a..f57e4bd68d 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -1,28 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs" # # Load default params for this platform @@ -52,31 +30,35 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + # # Start MAVLink # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - # # Start PWM output # fmu mode_pwm -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -pwm -u 400 -m 0xff - -# Try to get an USB console -nshterm /dev/ttyACM0 & +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink -#exit +exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index 49483d14fe..a37c26ad1d 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs" # # Load default params for this platform @@ -28,74 +8,40 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 # -# Start MAVLink (depends on orb) +# Start MAVLink # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery # -# Disable px4io topic limiting +# Start and configure PX4IO interface # -px4io limit 200 - +sh /etc/init.d/rc.io + # -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) +# Load mixer # mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# pwm -u 400 -m 0xff -multirotor_att_control start - + # -# Start logging once armed +# Start common for all multirotors apps # -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index 5bb1491e91..eb9f82f771 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -1,86 +1,45 @@ #!nsh -# -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -echo "[init] doing PX4IOAR startup..." + +echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" # -# Start the ORB +# Load default params for this platform # -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + # # Configure PX4FMU for operation with PX4IOAR # fmu mode_gpio_serial -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Fire up the multi rotor attitude controller -# -multirotor_att_control start - # # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 - -# -# Start GPS capture -# -gps start # -# startup is done; we don't want the shell because we -# use the same UART for telemetry +# Start common for all multirotors apps # -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index a223ef7aab..44fbb79b7b 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -1,49 +1,47 @@ #!nsh + +echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" + # -# Flight startup script for PX4FMU on PX4IOAR carrier board. +# Load default params for this platform # - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +# +# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 + # # Configure PX4FMU for operation with PX4IOAR # fmu mode_gpio_serial + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 # # Start the sensors. # sh /etc/init.d/rc.sensors -# -# Start MAVLink and MAVLink Onboard (Flow Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - # # Start the commander. # @@ -73,25 +71,6 @@ flow_position_control start # Fire up the flow speed controller # flow_speed_control start - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 50842764a6..7b6509bf88 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,12 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on an F330 quad. -# -# -# Start the ORB (first app to start) -# -uorb start +echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" # # Load default params for this platform @@ -35,8 +29,7 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -47,71 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) +# Set PWM values for DJI ESCs # -if px4io start -then - # - # This sets a PWM right after startup (regardless of safety button) - # - px4io idle 900 900 900 900 - - pwm -u 400 -m 0xff - - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - - - - # - # The values are for spinning motors when armed using DJI ESCs - # - px4io min 1200 1200 1200 1200 - - # - # Upper limits could be higher, this is on the safe side - # - px4io max 1800 1800 1800 1800 - - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -else - # SOS - tone_alarm 6 -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -multirotor_att_control start +px4io idle 900 900 900 900 +px4io min 1200 1200 1200 1200 +px4io max 1800 1800 1800 1800 # -# Disable px4io topic limiting and start logging +# Load mixer # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 -fi +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs index 237bb4267c..b4f063e524 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -1,27 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad -# with stock DJI ESCs, motors and props. -# - -# disable USB and autostart -set USB no -set MODE custom -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi +echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad" # # Load default params for this platform @@ -47,11 +26,10 @@ then param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -62,77 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) -# -px4io start -pwm -u 400 -m 0xff - -# -# Allow PX4IO to recover from midair restarts. -# This is very unlikely, but quite safe and robust. -px4io recovery - -# -# This sets a PWM right after startup (regardless of safety button) +# Set PWM values for DJI ESCs # px4io idle 900 900 900 900 - -# -# The values are for spinning motors when armed using DJI ESCs -# px4io min 1180 1180 1180 1180 - -# -# Upper limits could be higher, this is on the safe side -# px4io max 1800 1800 1800 1800 # # Load the mixer for a quad with wide arms # mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start # -# Start the controllers (depends on orb) +# Set PWM output frequency # -multirotor_att_control start +pwm -u 400 -m 0xff # -# Disable px4io topic limiting and start logging +# Start common for all multirotors apps # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 - if blinkm start - then - blinkm systemstate - fi -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 - if rgbled start - then - #rgbled systemstate - fi -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 5090b98a42..c9d5d66326 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -28,39 +8,18 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - # # Start MAVLink (depends on orb) # @@ -106,16 +65,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 5090b98a42..0deffe3f17 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom" # # Load default params for this platform @@ -28,76 +8,50 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - + # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +sh /etc/init.d/rc.io # # Set actuator limit to 100 Hz update (50 Hz PWM) px4io limit 100 # -# Start the sensors (depends on orb, px4io) +# Start the commander +# +commander start + +# +# Start the sensors # sh /etc/init.d/rc.sensors # -# Start GPS interface (depends on orb) +# Start GPS interface # gps start # -# Start the attitude estimator (depends on orb) +# Start the attitude estimator # kalman_demo start @@ -106,16 +60,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 5742d685aa..2890f43bec 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -1,26 +1,4 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi # # Load default params for this platform @@ -28,65 +6,34 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 10 = ground rover # param set MAV_TYPE 10 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 +# +# Start and configure PX4IO interface +# +sh /etc/init.d/rc.io + # # Start the commander (depends on orb, mavlink) # commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery -# -# Disable px4io topic limiting -# -px4io limit 200 - # # Start the sensors (depends on orb, px4io) # @@ -107,16 +54,3 @@ attitude_estimator_ekf start # md25 start 3 0x58 segway start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 similarity index 50% rename from ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 rename to ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index a7ffffaeec..2c82180131 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -1,28 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" # # Load default params for this platform @@ -52,8 +30,7 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -62,55 +39,26 @@ param set MAV_TYPE 2 # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start # -# Start GPS interface (depends on orb) +# Start PWM output # -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start -# -# Start the position estimator -# -position_estimator_inav start - -echo "[init] starting PWM output" fmu mode_pwm + +# +# Load mixer +# mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# pwm -u 400 -m 0xff - -# -# Start attitude control -# -multirotor_att_control start # -# Start position control +# Start common for all multirotors apps # -multirotor_pos_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 +sh /etc/init.d/rc.multirotor -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io new file mode 100644 index 0000000000..85f00e582e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -0,0 +1,23 @@ +# +# Start PX4IO interface (depends on orb, commander) +# +if px4io start +then + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery + + # + # Disable px4io topic limiting + # + if [ $BOARD == fmuv1 ] + then + px4io limit 200 + else + px4io limit 400 + fi +else + # SOS + tone_alarm 6 +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 09c2d00d19..dc4be8055a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,5 +5,10 @@ if [ -d /fs/microsd ] then - sdlog start + if [ $BOARD == fmuv1 ] + then + sdlog2 start -r 50 -a -b 16 + else + sdlog2 start -r 200 -a -b 16 + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 81184f3632..73f1b37429 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -37,13 +37,3 @@ multirotor_att_control start # Start position control # multirotor_pos_control start - -# -# Start logging -# -if [ $BOARD == fmuv1 ] -then - sdlog2 start -r 50 -a -b 16 -else - sdlog2 start -r 200 -a -b 16 -fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b2780..d92a3ce5e3 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -57,162 +57,165 @@ fi if [ $MODE == autostart ] then - -# -# Start terminal -# -if sercon -then - echo "USB connected" -else - # second attempt - sercon & -fi - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -if ramtron start -then - param select /ramtron/params - if [ -f /ramtron/params ] + # + # Start terminal + # + if sercon then - param load /ramtron/params - fi -else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] - then - param load /fs/microsd/params - fi -fi - -# -# Start system state indicator -# -if rgbled start -then - echo "Using external RGB Led" -else - if blinkm start - then - blinkm systemstate - fi -fi - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - then - echo "No newer version, skipping upgrade." + echo "USB connected" else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + # second attempt + sercon & + fi + + # + # Start the ORB (first app to start) + # + uorb start + + # + # Load microSD params + # + if ramtron start + then + param select /ramtron/params + if [ -f /ramtron/params ] then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + param load /ramtron/params + fi + else + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + param load /fs/microsd/params + fi + fi + + # + # Start system state indicator + # + if rgbled start + then + echo "Using external RGB Led" + else + if blinkm start + then + blinkm systemstate + fi + fi + + # + # Start logging + # + sh /etc/init.d/rc.logging + + # + # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) + # + if [ -f /fs/microsd/px4io.bin ] + then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + then + echo "No newer version, skipping upgrade." else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log - echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log + echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + fi fi fi -fi - -# -# Check if auto-setup from one of the standard scripts is wanted -# SYS_AUTOSTART = 0 means no autostart (default) -# -if param compare SYS_AUTOSTART 1 -then - sh /etc/init.d/01_fmu_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 2 -then - sh /etc/init.d/02_io_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 8 -then - sh /etc/init.d/08_ardrone - set MODE custom -fi - -if param compare SYS_AUTOSTART 9 -then - sh /etc/init.d/09_ardrone_flow - set MODE custom -fi - -if param compare SYS_AUTOSTART 10 -then - sh /etc/init.d/10_io_f330 - set MODE custom -fi - -if param compare SYS_AUTOSTART 15 -then - sh /etc/init.d/15_io_tbs - set MODE custom -fi - -if param compare SYS_AUTOSTART 30 -then - sh /etc/init.d/30_io_camflyer - set MODE custom -fi - -if param compare SYS_AUTOSTART 31 -then - sh /etc/init.d/31_io_phantom - set MODE custom -fi - -# Try to get an USB console -nshterm /dev/ttyACM0 & - -# If none of the autostart scripts triggered, get a minimal setup -if [ $MODE == autostart ] -then - # Telemetry port is on both FMU boards ttyS1 - mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 - - # Start commander - commander start - - # Start px4io if present - if px4io start + + # + # Check if auto-setup from one of the standard scripts is wanted + # SYS_AUTOSTART = 0 means no autostart (default) + # + if param compare SYS_AUTOSTART 1 then - echo "PX4IO driver started" - else - if fmu mode_serial - then - echo "FMU driver started" - fi + sh /etc/init.d/01_fmu_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 2 + then + sh /etc/init.d/02_io_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 8 + then + sh /etc/init.d/08_ardrone + set MODE custom + fi + + if param compare SYS_AUTOSTART 9 + then + sh /etc/init.d/09_ardrone_flow + set MODE custom + fi + + if param compare SYS_AUTOSTART 10 + then + sh /etc/init.d/10_io_f330 + set MODE custom + fi + + if param compare SYS_AUTOSTART 15 + then + sh /etc/init.d/15_io_tbs + set MODE custom + fi + + if param compare SYS_AUTOSTART 30 + then + sh /etc/init.d/30_io_camflyer + set MODE custom + fi + + if param compare SYS_AUTOSTART 31 + then + sh /etc/init.d/31_io_phantom + set MODE custom + fi + + # Try to get an USB console + nshterm /dev/ttyACM0 & + + # If none of the autostart scripts triggered, get a minimal setup + if [ $MODE == autostart ] + then + # Telemetry port is on both FMU boards ttyS1 + mavlink start -b 57600 -d /dev/ttyS1 + usleep 5000 + + # Start commander + commander start + + # Start px4io if present + if px4io start + then + echo "PX4IO driver started" + else + if fmu mode_serial + then + echo "FMU driver started" + fi + fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - -fi - # End of autostart fi From 725bb7697c9fc85ac95fdadc9d2fd2ef5b9848f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 20:17:42 +0200 Subject: [PATCH 20/40] Minor fix in "set mode" command handling. --- src/modules/commander/commander.cpp | 13 ++++++------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f7ac243412..74cd5e36a4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -323,11 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - union px4_custom_mode custom_mode; - custom_mode.data_float = cmd->param2; + uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -357,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index eb28af1a10..af43542da4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -201,7 +201,7 @@ handle_message(mavlink_message_t *msg) custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = custom_mode.data_float; + vcmd.param2 = custom_mode.main_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; From 548f322493fae95c41f3769192fa0c9b28d44d26 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Aug 2013 22:43:01 +0200 Subject: [PATCH 21/40] Added a simple unit test framework and initial testing some of the commander state machines. --- makefiles/config_px4fmu-v1_default.mk | 7 + makefiles/config_px4fmu-v2_default.mk | 6 + .../commander_tests/commander_tests.cpp | 57 ++++ .../commander/commander_tests/module.mk | 41 +++ .../state_machine_helper_test.cpp | 248 ++++++++++++++++++ .../state_machine_helper_test.h | 43 +++ src/modules/test/foo.c | 4 - src/modules/test/module.mk | 4 - src/modules/unit_test/module.mk | 39 +++ src/modules/unit_test/unit_test.cpp | 65 +++++ src/modules/unit_test/unit_test.h | 93 +++++++ 11 files changed, 599 insertions(+), 8 deletions(-) create mode 100644 src/modules/commander/commander_tests/commander_tests.cpp create mode 100644 src/modules/commander/commander_tests/module.mk create mode 100644 src/modules/commander/commander_tests/state_machine_helper_test.cpp create mode 100644 src/modules/commander/commander_tests/state_machine_helper_test.h delete mode 100644 src/modules/test/foo.c delete mode 100644 src/modules/test/module.mk create mode 100644 src/modules/unit_test/module.mk create mode 100644 src/modules/unit_test/unit_test.cpp create mode 100644 src/modules/unit_test/unit_test.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a556d0b073..245fe8415d 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -90,6 +90,13 @@ MODULES += examples/flow_speed_control # MODULES += modules/sdlog2 +# +# Unit tests +# +MODULES += modules/unit_test +MODULES += modules/commander/commander_tests + + # # Library modules # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 20dbe717fd..c5131262b0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -83,6 +83,12 @@ MODULES += modules/multirotor_pos_control # MODULES += modules/sdlog2 +# +# Unit tests +# +MODULES += modules/unit_test +MODULES += modules/commander/commander_tests + # # Library modules # diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp new file mode 100644 index 0000000000..f1a9674aa2 --- /dev/null +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * 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 commander_tests.cpp + * Commander unit tests. + * + */ + +#include + +#include "state_machine_helper_test.h" + +extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); + + +int commander_tests_main(int argc, char *argv[]) +{ + state_machine_helper_test(); + //state_machine_test(); + + return 0; +} diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk new file mode 100644 index 0000000000..df9b7ac4b0 --- /dev/null +++ b/src/modules/commander/commander_tests/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. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = commander_tests +SRCS = commander_tests.cpp \ + state_machine_helper_test.cpp \ + ../state_machine_helper.cpp diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp new file mode 100644 index 0000000000..7101b455ae --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (C) 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 state_machine_helper_test.cpp + * System state machine unit test. + * + */ + +#include "state_machine_helper_test.h" + +#include "../state_machine_helper.h" +#include + +class StateMachineHelperTest : public UnitTest +{ +public: + StateMachineHelperTest(); + virtual ~StateMachineHelperTest(); + + virtual const char* run_tests(); + +private: + const char* arming_state_transition_test(); + const char* arming_state_transition_arm_disarm_test(); + const char* main_state_transition_test(); + const char* is_safe_test(); +}; + +StateMachineHelperTest::StateMachineHelperTest() { +} + +StateMachineHelperTest::~StateMachineHelperTest() { +} + +const char* +StateMachineHelperTest::arming_state_transition_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // Identical states. + status.arming_state = ARMING_STATE_INIT; + new_arming_state = ARMING_STATE_INIT; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + + // INIT to STANDBY. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = true; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("transition: init to standby", + TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("ready to arm", armed.ready_to_arm); + + // INIT to STANDBY, sensors not initialized. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = false; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("no transition: sensors not initialized", + TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("not ready to arm", !armed.ready_to_arm); + + return 0; +} + +const char* +StateMachineHelperTest::arming_state_transition_arm_disarm_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // TODO(sjwilks): ARM then DISARM. + return 0; +} + +const char* +StateMachineHelperTest::main_state_transition_test() +{ + struct vehicle_status_s current_state; + main_state_t new_main_state; + + // Identical states. + current_state.main_state = MAIN_STATE_MANUAL; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // AUTO to MANUAL. + current_state.main_state = MAIN_STATE_AUTO; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("transition changed: auto to manual", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to SEATBELT. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = true; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("tranisition: manual to seatbelt", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + + // MANUAL to SEATBELT, invalid local altitude. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = false; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("no transition: invalid local altitude", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to EASY. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = true; + new_main_state = MAIN_STATE_EASY; + mu_assert("transition: manual to easy", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + + // MANUAL to EASY, invalid local position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = false; + new_main_state = MAIN_STATE_EASY; + mu_assert("no transition: invalid position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to AUTO. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = true; + new_main_state = MAIN_STATE_AUTO; + mu_assert("transition: manual to auto", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + + // MANUAL to AUTO, invalid global position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = false; + new_main_state = MAIN_STATE_AUTO; + mu_assert("no transition: invalid global position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + return 0; +} + +const char* +StateMachineHelperTest::is_safe_test() +{ + struct vehicle_status_s current_state; + struct safety_s safety; + struct actuator_armed_s armed; + + armed.armed = false; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + + armed.armed = false; + armed.lockdown = true; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = false; + safety.safety_off = false; + mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + + return 0; +} + +const char* +StateMachineHelperTest::run_tests() +{ + mu_run_test(arming_state_transition_test); + mu_run_test(arming_state_transition_arm_disarm_test); + mu_run_test(main_state_transition_test); + mu_run_test(is_safe_test); + + return 0; +} + +void +state_machine_helper_test() +{ + StateMachineHelperTest* test = new StateMachineHelperTest(); + test->UnitTest::print_results(test->run_tests()); +} + diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h new file mode 100644 index 0000000000..339b58d22b --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (C) 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 state_machine_helper_test.h + */ + +#ifndef STATE_MACHINE_HELPER_TEST_H_ +#define STATE_MACHINE_HELPER_TEST_ + +void state_machine_helper_test(); + +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ \ No newline at end of file diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c deleted file mode 100644 index ff6af031f7..0000000000 --- a/src/modules/test/foo.c +++ /dev/null @@ -1,4 +0,0 @@ -int test_main(void) -{ - return 0; -} \ No newline at end of file diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk deleted file mode 100644 index abf80eedf7..0000000000 --- a/src/modules/test/module.mk +++ /dev/null @@ -1,4 +0,0 @@ - -MODULE_NAME = test -SRCS = foo.c - diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk new file mode 100644 index 0000000000..f00b0f5926 --- /dev/null +++ b/src/modules/unit_test/module.mk @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +# +# Makefile to build the unit test library. +# + +SRCS = unit_test.cpp + diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp new file mode 100644 index 0000000000..64ee544a27 --- /dev/null +++ b/src/modules/unit_test/unit_test.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 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 unit_test.cpp + * A unit test library. + * + */ + +#include "unit_test.h" + +#include + + +UnitTest::UnitTest() +{ +} + +UnitTest::~UnitTest() +{ +} + +void +UnitTest::print_results(const char* result) +{ + if (result != 0) { + warnx("Failed: %s:%d", mu_last_test(), mu_line()); + warnx("%s", result); + } else { + warnx("ALL TESTS PASSED"); + warnx(" Tests run : %d", mu_tests_run()); + warnx(" Assertion : %d", mu_assertion()); + } +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h new file mode 100644 index 0000000000..5d4c3e46d8 --- /dev/null +++ b/src/modules/unit_test/unit_test.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 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 unit_test.h + * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). + * + */ + +#ifndef UNIT_TEST_H_ +#define UNIT_TEST_ + +#include + + +class __EXPORT UnitTest +{ + +public: +#define xstr(s) str(s) +#define str(s) #s +#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } + +INLINE_GLOBAL(int, mu_tests_run) +INLINE_GLOBAL(int, mu_assertion) +INLINE_GLOBAL(int, mu_line) +INLINE_GLOBAL(const char*, mu_last_test) + +#define mu_assert(message, test) \ + do \ + { \ + if (!(test)) \ + return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ + else \ + mu_assertion()++; \ + } while (0) + + +#define mu_run_test(test) \ +do \ +{ \ + const char *message; \ + mu_last_test() = #test; \ + mu_line() = __LINE__; \ + message = test(); \ + mu_tests_run()++; \ + if (message) \ + return message; \ +} while (0) + + +public: + UnitTest(); + virtual ~UnitTest(); + + virtual const char* run_tests() = 0; + virtual void print_results(const char* result); +}; + + + +#endif /* UNIT_TEST_H_ */ \ No newline at end of file From e25f2ff44f9579da3e5bef1e6d1baa3822ec40df Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Aug 2013 22:54:31 +0200 Subject: [PATCH 22/40] Whitespace and formatting cleanup. --- makefiles/config_px4fmu-v1_default.mk | 1 - src/modules/commander/commander_tests/commander_tests.cpp | 8 +++----- src/modules/commander/commander_tests/module.mk | 2 +- .../commander_tests/state_machine_helper_test.cpp | 1 - .../commander/commander_tests/state_machine_helper_test.h | 3 ++- src/modules/unit_test/unit_test.h | 2 +- 6 files changed, 7 insertions(+), 10 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 245fe8415d..bb0c1550f2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -96,7 +96,6 @@ MODULES += modules/sdlog2 MODULES += modules/unit_test MODULES += modules/commander/commander_tests - # # Library modules # diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index f1a9674aa2..6e72cf0d9c 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -1,10 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes + * Author: Simon Wilks * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +34,8 @@ /** * @file commander_tests.cpp - * Commander unit tests. + * Commander unit tests. Run the tests as follows: + * nsh> commander_tests * */ diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk index df9b7ac4b0..4d10275d1e 100644 --- a/src/modules/commander/commander_tests/module.mk +++ b/src/modules/commander/commander_tests/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 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/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 7101b455ae..40bedd9f31 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -245,4 +245,3 @@ state_machine_helper_test() StateMachineHelperTest* test = new StateMachineHelperTest(); test->UnitTest::print_results(test->run_tests()); } - diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index 339b58d22b..10a68e6028 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 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 @@ -40,4 +41,4 @@ void state_machine_helper_test(); -#endif /* STATE_MACHINE_HELPER_TEST_H_ */ \ No newline at end of file +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 5d4c3e46d8..3020734f4b 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -90,4 +90,4 @@ public: -#endif /* UNIT_TEST_H_ */ \ No newline at end of file +#endif /* UNIT_TEST_H_ */ From 07f7fd1585dab0a0256b0e8dda48ea1a1bd65388 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 25 Aug 2013 22:26:47 -0700 Subject: [PATCH 23/40] Fix the firmware build rules so that we always know how to build all the firmwares and thus we can have dependencies between FMU and IO firmware handled a little more sensibly. --- Makefile | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/Makefile b/Makefile index cbf0da6c97..83a6f3ce9f 100644 --- a/Makefile +++ b/Makefile @@ -40,14 +40,16 @@ export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ include $(PX4_BASE)makefiles/setup.mk # -# Canned firmware configurations that we build. +# Canned firmware configurations that we (know how to) build. # -CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +CONFIGS ?= $(KNOWN_CONFIGS) # -# Boards that we build NuttX export kits for. +# Boards that we (know how to) build NuttX export kits for. # -BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +BOARDS ?= $(KNOWN_BOARDS) # # Debugging @@ -87,10 +89,11 @@ endif # # Built products # -STAGED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) -FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) +DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) +STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: $(STAGED_FIRMWARES) +all: $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. @@ -122,7 +125,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: $(FIRMWARE_GOAL) # -# Make FMU firmwares depend on pre-packaged IO binaries. +# Make FMU firmwares depend on the corresponding IO firmware. # # This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency # and forces the _default config in all cases. There has to be a better way to do this... From c5731bbc3f29361f3d50ecc54d44a521d2441a48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 09:12:17 +0200 Subject: [PATCH 24/40] TAKEOFF implemented for multirotors, added altitude check to waypoint navigation. --- .../att_pos_estimator_ekf/KalmanNav.cpp | 2 +- src/modules/commander/commander.cpp | 14 ++- .../commander/state_machine_helper.cpp | 1 + src/modules/mavlink/orb_listener.c | 2 +- src/modules/mavlink/waypoints.c | 40 +++--- .../multirotor_pos_control.c | 115 +++++++++++------- .../position_estimator_inav_main.c | 3 +- .../uORB/topics/vehicle_control_mode.h | 2 + .../uORB/topics/vehicle_global_position.h | 3 +- .../uORB/topics/vehicle_local_position.h | 2 + 10 files changed, 108 insertions(+), 76 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 191d20f306..33879892ee 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -325,7 +325,7 @@ void KalmanNav::updatePublications() _pos.vx = vN; _pos.vy = vE; _pos.vz = vD; - _pos.hdg = psi; + _pos.yaw = psi; // attitude publication _att.timestamp = _pubTimeStamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74cd5e36a4..39d74e37aa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -210,7 +210,7 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -1214,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) } /* evaluate the navigation state machine */ - transition_result_t res = check_navigation_state_machine(&status, &control_mode); + transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1572,7 +1572,7 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) { transition_result_t res = TRANSITION_DENIED; @@ -1592,8 +1592,12 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + /* check if takeoff completed */ + if (local_pos->z < -5.0f && !status.condition_landed) { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } else { + res = TRANSITION_NOT_CHANGED; + } } else { if (current_status->condition_landed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 80ee3db233..fe7e8cc536 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + control_mode->auto_state = current_status->navigation_state; navigation_state_changed = true; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index dcdc03281a..53d86ec005 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -341,7 +341,7 @@ l_global_position(const struct listener *l) int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + 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, diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index eea928a170..16a7c2d355 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) */ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) { - //TODO: implement for z once altidude contoller is implemented - static uint16_t counter; -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; + double current_x_rad = wp->x / 180.0 * M_PI; + double current_y_rad = wp->y / 180.0 * M_PI; double x_rad = lat / 180.0 * M_PI; double y_rad = lon / 180.0 * M_PI; @@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float const double radius_earth = 6371000.0; - return radius_earth * c; + float dxy = radius_earth * c; + float dz = alt - wp->z; + + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; @@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ // XXX TODO } - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - + if (dist >= 0.f && dist <= orbit) { wpm->pos_reached = true; - } - -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } + // check if required yaw reached + float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); + float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); + if (fabsf(yaw_err) < 0.05f) { + wpm->yaw_reached = true; + } } //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { if (wpm->current_active_wp_id < wpm->size) { mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); @@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ bool time_elapsed = false; - if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } - } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { time_elapsed = true; } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { time_elapsed = true; @@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); // } - check_waypoints_reached(now, global_position , local_position); + check_waypoints_reached(now, global_position, local_position); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a6ebeeacb1..e65792c76b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_int_xy = true; bool was_armed = false; bool reset_integral = true; + bool reset_auto_pos = true; hrt_abstime t_prev = 0; - /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + const float takeoff_alt_default = 10.0f; float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; @@ -414,50 +416,76 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, use global setpoint */ - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - global_pos_sp_reproject = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_reproject) { - /* update global setpoint projection */ - global_pos_sp_reproject = false; - - if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - - } else { - if (!local_pos_sp_valid) { - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = -takeoff_alt_default; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { + // TODO + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); } - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } + att_sp.yaw_body = global_pos_sp.yaw; + reset_auto_pos = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + global_pos_sp_reproject = true; } /* reset setpoints after non-manual modes */ @@ -575,6 +603,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_int_z = true; reset_int_xy = true; global_pos_sp_reproject = true; + reset_auto_pos = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ 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 1e20f9de9d..af6a704a2e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -591,6 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.landed = landed; + local_pos.yaw = att.yaw; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); @@ -609,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { @@ -623,6 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 4f4db5dbcb..e15ddde269 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -82,6 +82,8 @@ struct vehicle_control_mode_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ + + uint8_t auto_state; // TEMP navigation state for AUTO modes }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 822c323cfc..0fc0ed5c97 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -72,8 +72,7 @@ struct vehicle_global_position_s float vx; /**< Ground X velocity, m/s in NED */ float vy; /**< Ground Y velocity, m/s in NED */ float vz; /**< Ground Z velocity, m/s in NED */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - + float yaw; /**< Compass heading in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 26e8f335b5..31a0e632b2 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -67,6 +67,8 @@ struct vehicle_local_position_s float vx; /**< Ground X Speed (Latitude), m/s in NED */ float vy; /**< Ground Y Speed (Longitude), m/s in NED */ float vz; /**< Ground Z Speed (Altitude), m/s in NED */ + /* Heading */ + float yaw; /* Reference position in GPS / WGS84 frame */ bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ From dfde02c82507d183fdc16aa2d45cb8d9cdf6ff0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 11:53:52 +0200 Subject: [PATCH 25/40] Startup scripts fixup, fixed unmatched dependencies --- ROMFS/px4fmu_common/init.d/30_io_camflyer | 9 +++++++-- ROMFS/px4fmu_common/init.d/31_io_phantom | 7 ++++++- ROMFS/px4fmu_common/init.d/rc.multirotor | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 11 +++-------- 4 files changed, 21 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index c9d5d66326..6a0bd4da82 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,6 +1,6 @@ #!nsh -cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" +echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -49,6 +49,11 @@ px4io limit 100 # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface (depends on orb) @@ -64,4 +69,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 0deffe3f17..7183138622 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -44,6 +44,11 @@ commander start # Start the sensors # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface @@ -59,4 +64,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 73f1b37429..e3638e3d1f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -8,6 +8,11 @@ # sh /etc/init.d/rc.sensors +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + # # Start the commander. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6a82acdcc3..c4abd07d2e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -103,11 +103,9 @@ then blinkm systemstate fi fi - - # - # Start logging - # - sh /etc/init.d/rc.logging + + # Try to get an USB console + nshterm /dev/ttyACM0 & # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) @@ -183,9 +181,6 @@ then set MODE custom fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then From 3157285254381827bc0312bfb413ff6823feee3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 13:26:01 +0200 Subject: [PATCH 26/40] Increased the number of max files descriptors considerably --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c65..56ad46c3f5 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_STREAMS=50 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 3e2a481087..f99973aec5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_STREAMS=50 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 From 00a2a0370eedf84f68dcda5995f4e34495aaf887 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 12:43:36 +0200 Subject: [PATCH 27/40] accelerometer_calibration fix --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 82df7c37d6..7a7fa6f4e4 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -298,7 +298,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ - int64_t still_time = 2000000; + hrt_abstime still_time = 2000000; struct pollfd fds[1]; fds[0].fd = sub_sensor_combined; fds[0].events = POLLIN; @@ -342,7 +342,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { t_timeout = t + timeout; } else { /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { + if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } From bf9282c9882882a5fc4adf680a6ecc5e655bb0e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:10 +0200 Subject: [PATCH 28/40] position_estimator_inav: requre EPH < 5m to set GPS reference --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 af6a704a2e..bc6e0b0209 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { From baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:55 +0200 Subject: [PATCH 29/40] commander: do AUTO_MISSION after takeoff --- src/modules/commander/commander.cpp | 58 ++++++++++++++--------------- 1 file changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39d74e37aa..d900086331 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* check if takeoff completed */ - if (local_pos->z < -5.0f && !status.condition_landed) { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } else { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -5.0f || status.condition_landed) { res = TRANSITION_NOT_CHANGED; + break; } + } + /* check again, state can be changed */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* switch to mission in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } } else { From 7326f8a4215fe736aedd2e2cdb2ab51d06e20f80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:53:30 +0200 Subject: [PATCH 30/40] multirotor_pos_control: fixes, set local_position_sp.yaw --- .../multirotor_pos_control.c | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e65792c76b..385892f04e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -405,30 +405,38 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for loiter after position controlled mode */ local_pos_sp_valid = control_mode.flag_control_position_enabled; + reset_auto_pos = true; + /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; } else { /* non-manual mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; } @@ -462,6 +470,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; + att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -472,15 +482,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } - - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } - att_sp.yaw_body = global_pos_sp.yaw; reset_auto_pos = true; } @@ -493,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_sp_z = true; } + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; From e88d63ef272124e8c0ee9574506d14866feadb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 15:03:58 +0200 Subject: [PATCH 31/40] Increased USB buffer size to cope with fast transfers --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index b6f14490e0..3e02c204e1 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -564,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" From b29d13347a10156bf1690b67938e2850d4e1e4c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 22:08:56 +0200 Subject: [PATCH 32/40] position_estimator_inav: reset reference altitude on arming. --- .../position_estimator_inav_main.c | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) 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 bc6e0b0209..ef13ade886 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s + +static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const uint32_t updates_counter_len = 1000000; +static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); - uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = hrt_absolute_time(); - uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* acceleration in NED frame */ float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; @@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { // new ground level baro_alt0 += sonar_corr; - warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); @@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + baro_alt0 -= z_est[0]; + z_est[0] = 0.0f; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + } + /* accel bias correction, now only for Z * not strictly correct, but stable and works */ accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; @@ -629,6 +638,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } + flag_armed = armed.armed; } warnx("exiting."); From bfd0444cb30bf2db3f19a6a1c052d85c2270a090 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 07:49:36 +0200 Subject: [PATCH 33/40] Revert "Increased the number of max files descriptors considerably" This reverts commit 3157285254381827bc0312bfb413ff6823feee3b. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 56ad46c3f5..a7a6725c65 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 -CONFIG_NFILE_STREAMS=50 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index f99973aec5..3e2a481087 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 -CONFIG_NFILE_STREAMS=50 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 From 7b42d7a0470aaa50e9a8ec5852007ea52165a7ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 07:50:15 +0200 Subject: [PATCH 34/40] Made number of streams more reasonable --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c65..b8b98b1496 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -406,7 +406,7 @@ CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 3e2a481087..e507c89baf 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -452,7 +452,7 @@ CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 From 94d8ec4a1c1f052a50f4871db7445fb6454057d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 09:48:22 +0200 Subject: [PATCH 35/40] Calibration message cleanup --- .../commander/accelerometer_calibration.cpp | 17 ++++++------- src/modules/commander/gyro_calibration.cpp | 15 +++++++---- src/modules/commander/mag_calibration.cpp | 25 +++++++++++++------ 3 files changed, 35 insertions(+), 22 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a7fa6f4e4..e1616acd04 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -219,7 +219,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float bool done = true; char str[60]; int str_ptr; - str_ptr = sprintf(str, "keep vehicle still:"); + str_ptr = sprintf(str, "keep the vehicle still in one of these axes:"); unsigned old_done_count = done_count; done_count = 0; for (int i = 0; i < 6; i++) { @@ -236,22 +236,21 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float if (done) break; - mavlink_log_info(mavlink_fd, str); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) return ERROR; if (data_collected[orient]) { - sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); + sprintf(str, "%s done, please rotate to a different axis", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); continue; } // sprintf(str, - mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + str_ptr = sprintf(str, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -265,7 +264,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float float accel_T[3][3]; int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; } @@ -337,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "still..."); + mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; } else { @@ -352,7 +351,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving..."); + mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); t_still = 0; } } @@ -364,7 +363,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); + mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); return -1; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 9cd2f3a19c..33566d4e53 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -58,7 +58,7 @@ static const int ERROR = -1; int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); const int calibration_count = 5000; @@ -84,6 +84,8 @@ int do_gyro_calibration(int mavlink_fd) close(fd); + unsigned poll_errcount = 0; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -93,16 +95,19 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { + if (poll_ret > 0) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); gyro_offset[0] += raw.gyro_rad_s[0]; gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 42f0190c7e..e386160271 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -61,7 +61,7 @@ static const int ERROR = -1; int do_mag_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait."); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -123,6 +123,10 @@ int do_mag_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + + unsigned poll_errcount = 0; + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -137,7 +141,7 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); @@ -158,7 +162,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { + if (poll_ret > 0) { orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; @@ -190,11 +194,16 @@ int do_mag_calibration(int mavlink_fd) calibration_counter++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; + } else { + poll_errcount++; } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + return ERROR; + } + + } float sphere_x; @@ -276,7 +285,7 @@ int do_mag_calibration(int mavlink_fd) (double)mscale.y_scale, (double)mscale.z_scale); mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; From 665a23259269d870e958543c6e6517323793c1dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 10:15:17 +0200 Subject: [PATCH 36/40] More calibration polishing --- .../commander/accelerometer_calibration.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index e1616acd04..30ac07e332 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -217,20 +217,23 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float while (true) { bool done = true; - char str[60]; - int str_ptr; - str_ptr = sprintf(str, "keep the vehicle still in one of these axes:"); unsigned old_done_count = done_count; done_count = 0; + for (int i = 0; i < 6; i++) { if (!data_collected[i]) { - str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; - } else { - done_count++; } } + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", + (!data_collected[0]) ? "x+ " : "", + (!data_collected[0]) ? "x- " : "", + (!data_collected[0]) ? "y+ " : "", + (!data_collected[0]) ? "y- " : "", + (!data_collected[0]) ? "z+ " : "", + (!data_collected[0]) ? "z- " : ""); + if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); @@ -242,19 +245,17 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float return ERROR; if (data_collected[orient]) { - sprintf(str, "%s done, please rotate to a different axis", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); continue; } - // sprintf(str, mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; tune_neutral(); } From 9c58d2c5c6ef96f9ece7f62d5f02d01d8b1e316e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Aug 2013 18:07:31 +1000 Subject: [PATCH 37/40] airspeed: retry initial I2C probe 4 times this fixes a problem with detecting a MS4525D0 at boot --- src/drivers/airspeed/airspeed.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5a8157debc..277d8249a3 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -146,7 +146,14 @@ out: int Airspeed::probe() { - return measure(); + /* on initial power up the device needs more than one retry + for detection. Once it is running then retries aren't + needed + */ + _retries = 4; + int ret = measure(); + _retries = 0; + return ret; } int From 33c73429098fee0650bdf2042a2c85e18975afca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 10:36:43 +0200 Subject: [PATCH 38/40] Minor fixes for calibration, UI language much more readable now --- src/modules/commander/accelerometer_calibration.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 30ac07e332..ed6707f045 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -228,11 +228,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", - (!data_collected[0]) ? "x- " : "", - (!data_collected[0]) ? "y+ " : "", - (!data_collected[0]) ? "y- " : "", - (!data_collected[0]) ? "z+ " : "", - (!data_collected[0]) ? "z- " : ""); + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); From b9d6981cee9878158435b9b1daa955d5b25c0389 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 13:40:18 +0200 Subject: [PATCH 39/40] multirotor_att_control: yaw control bug fixed --- .../multirotor_att_control_main.c | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 09955ec507..b3669d9ff6 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -147,8 +147,6 @@ mc_thread_main(int argc, char *argv[]) warnx("starting"); /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; bool control_yaw_position = true; bool reset_yaw_sp = true; bool failsafe_first_time = true; @@ -213,6 +211,11 @@ mc_thread_main(int argc, char *argv[]) /* set flag to safe value */ control_yaw_position = true; + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + reset_yaw_sp = true; + } + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ @@ -234,10 +237,12 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; + } else if (control_mode.flag_control_manual_enabled) { /* manual input */ if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { @@ -279,17 +284,8 @@ mc_thread_main(int argc, char *argv[]) } else { failsafe_first_time = true; - /* control yaw in all manual / assisted modes */ - /* set yaw if arming or switching to attitude stabilized mode */ - - if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) { - reset_yaw_sp = true; - } - - /* only move setpoint if manual input is != 0 */ - - // TODO review yaw restpoint reset - if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) { + /* only move yaw setpoint if manual input is != 0 and not landed */ + if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = false; rates_sp.yaw = manual.yaw; @@ -340,7 +336,14 @@ mc_thread_main(int argc, char *argv[]) rates_sp.thrust = manual.throttle; rates_sp.timestamp = hrt_absolute_time(); } + + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; } /* check if we should we reset integrals */ From 3380d40a7d9a8d1946510fab42abdc7a3a6f1525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 15:37:04 +0200 Subject: [PATCH 40/40] Tighter configs to save RAM --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a6f914a647..feb4a393df 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=24 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e507c89baf..e1bc867e53 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,7 +451,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=24 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4