diff options
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 184 | ||||
-rw-r--r-- | src/modules/commander/mag_calibration.cpp | 79 |
2 files changed, 150 insertions, 113 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f3876..247a2c5b8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include <uORB/topics/geofence_result.h> #include <uORB/topics/telemetry_status.h> #include <uORB/topics/vtol_vehicle_status.h> + #include <uORB/topics/vehicle_land_detected.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -241,6 +242,13 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + +/** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ void *commander_low_prio_loop(void *arg); @@ -560,7 +568,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd->param1 > 0.5f) { //XXX update state machine? @@ -716,6 +724,53 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s return true; } +/** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) +{ + //Need global position fix to be able to set home + if (!status.condition_global_position_valid) { + return; + } + + //Ensure that the GPS accuracy is good enough for intializing home + if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { + return; + } + + //Set home position + home.timestamp = hrt_absolute_time(); + home.lat = globalPosition.lat; + home.lon = globalPosition.lon; + home.alt = globalPosition.alt; + + home.x = localPosition.x; + home.y = localPosition.y; + home.z = localPosition.z; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (homePub > 0) { + orb_publish(ORB_ID(home_position), homePub, &home); + + } else { + homePub = orb_advertise(ORB_ID(home_position), &home); + } + + //Play tune first time we initialize HOME + if (!status.condition_home_position_valid) { + tune_positive(true); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; +} + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -904,7 +959,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; hrt_abstime last_idle_time = 0; - hrt_abstime start_time = 0; bool status_changed = true; bool param_init_forced = true; @@ -964,6 +1018,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); + /* Subscribe to land detector */ + int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + struct vehicle_land_detected_s land_detector; + memset(&land_detector, 0, sizeof(land_detector)); + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a @@ -1035,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; - start_time = hrt_absolute_time(); + const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1096,8 +1155,8 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1259,12 +1318,12 @@ int commander_thread_main(int argc, char *argv[]) } //Notify the user if the status of the safety switch changes - if(safety.safety_switch_available && previous_safety_off != safety.safety_off) { + if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { - if(safety.safety_off) { + if (safety.safety_off) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); - } - else { + + } else { tune_neutral(true); } @@ -1279,8 +1338,9 @@ int commander_thread_main(int argc, char *argv[]) /* vtol status changed */ orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; + /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { + if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1325,34 +1385,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* update home position */ - if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - tune_positive(true); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ /* hysteresis for EPH */ bool local_eph_good; @@ -1379,9 +1411,15 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + /* Update land detector */ + orb_check(land_detector_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); + } + if (status.condition_local_altitude_valid) { - if (status.condition_landed != local_position.landed) { - status.condition_landed = local_position.landed; + if (status.condition_landed != land_detector.landed) { + status.condition_landed = land_detector.landed; status_changed = true; if (status.condition_landed) { @@ -1401,7 +1439,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; @@ -1419,7 +1457,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("subsystem changed: %d\n", (int)info.subsystem_type); + //warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { @@ -1609,7 +1647,8 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums", + (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } } @@ -1710,9 +1749,9 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; - status.rc_signal_lost_timestamp=sp_man.timestamp; + status.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; } } @@ -1859,42 +1898,23 @@ int commander_thread_main(int argc, char *argv[]) } } + //Get current timestamp + const hrt_abstime now = hrt_absolute_time(); - hrt_abstime t1 = hrt_absolute_time(); + //First time home position update + if (!status.condition_home_position_valid) { + commander_set_home_position(home_pub, home, local_position, global_position); + } + + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + commander_set_home_position(home_pub, home, local_position, global_position); + } /* print new state */ if (arming_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); - - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - // TODO remove code duplication - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - } - arming_state_changed = false; } @@ -1915,11 +1935,14 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; + if (status.failsafe) { mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { mavlink_log_critical(mavlink_fd, "failsafe mode off"); } + failsafe_old = status.failsafe; } @@ -1932,13 +1955,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { set_control_mode(); - control_mode.timestamp = t1; + control_mode.timestamp = now; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - status.timestamp = t1; + status.timestamp = now; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed.timestamp = t1; + armed.timestamp = now; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1965,7 +1988,7 @@ int commander_thread_main(int argc, char *argv[]) if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { //Notify the user that it is safe to approach the vehicle - if(arm_tune_played) { + if (arm_tune_played) { tune_neutral(true); } @@ -2432,6 +2455,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } + break; default: @@ -2470,7 +2494,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(true); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7be8de9c6..53013fdb9 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,7 @@ static const char *sensor_name = "mag"; int do_mag_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "don't move system"); @@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd) /* erase old calibration */ int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (res != OK) { @@ -146,54 +150,60 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); - struct mag_report mag; - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + if (sub_mag < 0) { + mavlink_log_critical(mavlink_fd, "No mag found, abort"); + res = ERROR; + } else { + struct mag_report mag; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); + calibration_counter = 0U; - calibration_counter = 0; + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + int poll_ret = poll(fds, 1, 1000); - int poll_ret = poll(fds, 1, 1000); + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + calibration_counter++; - calibration_counter++; + if (calibration_counter % (calibration_maxcount / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } else { + poll_errcount++; } - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + close(sub_mag); } - - close(sub_mag); } float sphere_x; @@ -201,7 +211,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - if (res == OK) { + if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); @@ -253,6 +263,9 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ + if (param_set(param_find("SENS_MAG_ID"), &(device_id))) { + res = ERROR; + } if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; } |