diff options
Diffstat (limited to 'src')
45 files changed, 1364 insertions, 532 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5a8157deb..277d8249a 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 diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index b06920a76..ad21f7143 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]); } diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 191d20f30..33879892e 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/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ddd2e23d9..ed6707f04 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -133,9 +133,10 @@ 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"); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; @@ -176,11 +177,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 */ @@ -211,39 +212,50 @@ 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]; - 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; } } + + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", + (!data_collected[0]) ? "x+ " : "", + (!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); + 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]); - 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, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, 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, "meas result for %s: [ %.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(); } @@ -253,7 +265,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; } @@ -286,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; @@ -325,12 +337,12 @@ 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 { /* 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; } @@ -340,7 +352,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; } } @@ -352,7 +364,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/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 3275d9461..1cf9c0977 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -45,6 +45,6 @@ #include <stdint.h> -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 df08292e3..e414e5f70 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,7 +49,13 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> -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 92f5651ec..71c701fc2 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -41,6 +41,6 @@ #include <stdint.h> -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 d7515b3d9..3123c4087 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -47,8 +47,14 @@ #include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> -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 ac0f4be36..bc42bc6cf 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -41,6 +41,6 @@ #include <stdint.h> -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 17db0f9c8..d90008633 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <systemlib/cpuload.h> +#include <systemlib/rc_check.h> #include "px4_custom_mode.h" #include "commander_helper.h" @@ -209,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. @@ -322,10 +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; - uint32_t custom_mode = (uint32_t) 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; @@ -355,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 == PX4_CUSTOM_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 == PX4_CUSTOM_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 == PX4_CUSTOM_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 == PX4_CUSTOM_MODE_AUTO) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } @@ -502,7 +503,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 */ @@ -610,6 +617,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)); @@ -720,6 +729,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()); } } @@ -793,9 +805,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"); } } } @@ -1010,46 +1022,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) { @@ -1206,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 */ @@ -1564,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; @@ -1582,11 +1590,15 @@ 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->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* 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); @@ -1613,12 +1625,13 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v } } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + /* switch to mission in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } + } else { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); } - break; default: @@ -1628,6 +1641,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 */ @@ -1665,128 +1705,114 @@ 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; - /* 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 (((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 (is_safe(&status, &safety, &armed)) { + + 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; - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + break; - /* try to go to INIT/PREFLIGHT arming state */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - // 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; - } + int calib_ret = ERROR; - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - do_gyro_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* try to go to INIT/PREFLIGHT arming state */ - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - do_mag_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // 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; + } - } 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; + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - do_airspeed_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - break; - } + break; + } case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + 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); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } 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.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } - - break; } - - default: break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); - - } else { - tune_negative(); - - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - - } 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); - } + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; } /* send any requested ACKs */ 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 000000000..6e72cf0d9 --- /dev/null +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * 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. Run the tests as follows: + * nsh> commander_tests + * + */ + +#include <systemlib/err.h> + +#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 000000000..4d10275d1 --- /dev/null +++ b/src/modules/commander/commander_tests/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# 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 000000000..40bedd9f3 --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * 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 <unit_test/unit_test.h> + +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 000000000..10a68e602 --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * 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_ */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index f1afb0107..33566d4e5 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -50,10 +50,15 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> +/* 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"); + mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); const int calibration_count = 5000; @@ -79,6 +84,8 @@ void do_gyro_calibration(int mavlink_fd) close(fd); + unsigned poll_errcount = 0; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -88,17 +95,20 @@ void 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"); - return; + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); + return ERROR; } } @@ -137,18 +147,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 +189,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 +229,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 +240,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 +280,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 cd262507d..59c32a15e 100644 --- a/src/modules/commander/gyro_calibration.h +++ b/src/modules/commander/gyro_calibration.h @@ -41,6 +41,6 @@ #include <stdint.h> -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 9a25103f8..e38616027 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -53,10 +53,15 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> +/* 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"); + 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; @@ -95,6 +100,8 @@ void 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(); @@ -113,9 +120,13 @@ 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; } + 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) { @@ -130,9 +141,8 @@ void 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 axis.", 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; @@ -152,7 +162,7 @@ void 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; @@ -184,11 +194,16 @@ void 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; @@ -246,12 +261,15 @@ void 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(); 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", @@ -267,14 +285,14 @@ void 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"); - 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 fd2085f14..a101cd7b1 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -41,6 +41,6 @@ #include <stdint.h> -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/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 4d4859a5c..b60a7e0b9 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/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0de411713..fe87a3323 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -46,8 +46,13 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> +/* 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 6505c7364..9aa6faafa 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include <stdint.h> -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_ */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 80ee3db23..fe7e8cc53 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/mavlink.c b/src/modules/mavlink/mavlink.c index f39bbaa72..0a506b1a9 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 28f7af33c..af43542da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -71,6 +71,7 @@ #include <systemlib/err.h> #include <systemlib/airspeed.h> #include <mavlink/mavlink_log.h> +#include <commander/px4_custom_mode.h> __BEGIN_DECLS @@ -196,9 +197,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.main_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index c711b1803..53d86ec00 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, @@ -666,12 +666,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); + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + 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/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index eea928a17..16a7c2d35 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_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c057ef364..b3669d9ff 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[]) @@ -145,16 +147,14 @@ 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; /* 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 +176,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 +208,14 @@ 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; + + /* 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 */ @@ -225,47 +233,43 @@ 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); } + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; + } 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; - - } else { - att_sp.thrust = 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 > 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; } } @@ -273,46 +277,41 @@ 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; - - } else { - rc_loss_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 (failsafe_first_time) { 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; + } else { + failsafe_first_time = true; + + /* 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; - 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 +320,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 { @@ -336,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 */ @@ -367,6 +374,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,13 +382,10 @@ 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); - /* 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 */ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 12d16f7c7..c78232f11 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++; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a6ebeeacb..385892f04 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 <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/manual_control_setpoint.h> @@ -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; @@ -403,61 +405,95 @@ 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 */ - /* 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 (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; + } + } 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); + } - 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_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; + } + local_pos_sp.yaw = global_pos_sp.yaw; + att_sp.yaw_body = global_pos_sp.yaw; - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -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 { - 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); + 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; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + } - } 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); } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } + reset_auto_pos = true; + } - /* 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 (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + global_pos_sp_reproject = true; } /* reset setpoints after non-manual modes */ @@ -465,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]; @@ -575,6 +614,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 760e598bc..ef13ade88 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(); @@ -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) { @@ -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; @@ -591,6 +600,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 +619,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); } if (local_pos.z_valid) { @@ -623,11 +632,13 @@ 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; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } + flag_armed = armed.armed; } warnx("exiting."); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7f8648d95..2b21bb5a5 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; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index d19a7aec3..fd2a6318e 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 */ @@ -174,8 +173,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); @@ -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 ded39c465..e857b1c2f 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 94c744c03..843cda722 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 000000000..9d47d8000 --- /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 <nuttx/config.h> + +#include <stdio.h> +#include <fcntl.h> + +#include <systemlib/rc_check.h> +#include <systemlib/param/param.h> +#include <mavlink/mavlink_log.h> +#include <uORB/topics/rc_channels.h> + +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 000000000..e2238d151 --- /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/modules/test/foo.c b/src/modules/test/foo.c deleted file mode 100644 index ff6af031f..000000000 --- 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 abf80eedf..000000000 --- a/src/modules/test/module.mk +++ /dev/null @@ -1,4 +0,0 @@ - -MODULE_NAME = test -SRCS = foo.c - diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 4f4db5dbc..e15ddde26 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 f036c7223..0fc0ed5c9 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,18 +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 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 26e8f335b..31a0e632b 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) */ diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk new file mode 100644 index 000000000..f00b0f592 --- /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 000000000..64ee544a2 --- /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 <sjwilks@gmail.com> + * + * 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 <systemlib/err.h> + + +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 000000000..3020734f4 --- /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 <sjwilks@gmail.com> + * + * 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 <systemlib/err.h> + + +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_ */ diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e7d9ce85f..4c19dcffb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include <drivers/drv_baro.h> #include <mavlink/mavlink_log.h> +#include <systemlib/rc_check.h> __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; |