diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-13 21:25:08 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-13 21:25:08 +0200 |
commit | 5aea635a89c1a542c3874264b9369bb540e48c9e (patch) | |
tree | 4424840c2966e16a4e96e033a692903f86889f2f /src/modules | |
parent | f20a9c48732b578acd7164ae5c513c657092ab91 (diff) | |
parent | 178a3e8567b3e721771fffcb8f32df140ad1038b (diff) | |
download | px4-firmware-5aea635a89c1a542c3874264b9369bb540e48c9e.tar.gz px4-firmware-5aea635a89c1a542c3874264b9369bb540e48c9e.tar.bz2 px4-firmware-5aea635a89c1a542c3874264b9369bb540e48c9e.zip |
Merge remote-tracking branch 'upstream/master' into mtecs
Conflicts:
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
Diffstat (limited to 'src/modules')
107 files changed, 1012 insertions, 6973 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1cbdf9bf8..7180048ff 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix<3,3> board_rotation; + math::Matrix<3, 3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); math::Vector<3> accel_offs_vec(&accel_offs[0]); - math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); - math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec; + math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); @@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } - if (old_done_count != done_count) + if (old_done_count != done_count) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); + } - if (done) + if (done) { break; + } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", @@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); - if (d > still_thr2 * 8.0f) + if (d > still_thr2 * 8.0f) { d = still_thr2 * 8.0f; + } - if (d > accel_disp[i]) + if (d > accel_disp[i]) { accel_disp[i] = d; + } } /* still detector with hysteresis */ @@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 0; // [ g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 0; // [ g, 0, 0 ] + } if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 1; // [ -g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 1; // [ -g, 0, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 2; // [ 0, g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 2; // [ 0, g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 3; // [ 0, -g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 3; // [ 0, -g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) - return 4; // [ 0, 0, g ] + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { + return 4; // [ 0, 0, g ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) - return 5; // [ 0, 0, -g ] + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { + return 5; // [ 0, 0, -g ] + } mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); @@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { accel_sum[i] += sensor.accelerometer_m_s2[i]; + } count++; @@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp continue; } - if (errcount > samples_num / 10) + if (errcount > samples_num / 10) { return ERROR; + } } for (int i = 0; i < 3; i++) { @@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) - return ERROR; // Singular matrix + if (det == 0.0f) { + return ERROR; // Singular matrix + } dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; @@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; - if (mat_invert3(mat_A, mat_A_inv) != OK) + if (mat_invert3(mat_A, mat_A_inv) != OK) { return ERROR; + } /* copy results to accel_T */ for (int i = 0; i < 3; i++) { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index c8c7a42e7..5d21d89d0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd) bool paramreset_successful = false; int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } + close(fd); } @@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 53ed34f46..17d3d3dcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,9 +117,10 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */ -#define RC_TIMEOUT 500000 -#define DIFFPRESS_TIMEOUT 2000000 +#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ +#define RC_TIMEOUT 500000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -220,7 +221,7 @@ void print_status(); transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy); +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -232,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul int commander_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -260,8 +262,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { - if (!thread_running) + if (!thread_running) { errx(0, "commander already stopped"); + } thread_should_exit = true; @@ -303,8 +306,9 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); exit(1); @@ -363,20 +367,22 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - - // Transition the armed state. By passing mavlink_fd to arming_state_transition it will - // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); - if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); - } - - return arming_res; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will + // output appropriate error messages if the state cannot transition. + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + + if (arming_res == TRANSITION_CHANGED && mavlink_fd) { + mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; } bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) @@ -416,14 +422,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (hil_ret == OK) + if (hil_ret == OK) { ret = true; + } - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + // Transition the arming state + arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - if (arming_res == TRANSITION_CHANGED) + if (arming_res == TRANSITION_CHANGED) { ret = true; + } /* set main state */ transition_result_t main_res = TRANSITION_DENIED; @@ -434,13 +442,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { + /* ALTCTL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +463,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -465,8 +473,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (main_res == TRANSITION_CHANGED) + if (main_res == TRANSITION_CHANGED) { ret = true; + } if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -479,19 +488,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { + // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. + // We use an float epsilon delta to test float equality. + if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); - } else { - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); - if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } - } + + } else { + + // Flick to inair restore first if this comes from an onboard system + if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { + status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + } + + transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + } } break; @@ -519,7 +537,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command //XXX: to enable the parachute, a param needs to be set @@ -539,6 +557,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; + if (use_current) { /* use current position */ if (status->condition_global_position_valid) { @@ -582,6 +601,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -595,6 +615,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ answer_command(*cmd, result); @@ -628,8 +649,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTCTL"; + main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -741,8 +762,9 @@ int commander_thread_main(int argc, char *argv[]) bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; - uint64_t last_idle_time = 0; - uint64_t start_time = 0; + hrt_abstime last_idle_time = 0; + hrt_abstime start_time = 0; + hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -771,6 +793,9 @@ int commander_thread_main(int argc, char *argv[]) int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); + /* Init EPH and EPV */ + global_position.eph = 1000.0f; + global_position.epv = 1000.0f; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -873,6 +898,7 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); } + /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); @@ -913,6 +939,7 @@ int commander_thread_main(int argc, char *argv[]) /* disarm if safety is now on and still armed */ if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); } @@ -930,23 +957,31 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; + if (status.condition_global_position_valid) { if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { eph_epv_good = false; + + } else { + eph_epv_good = true; } } else { if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { eph_epv_good = true; + + } else { + eph_epv_good = false; } } + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -981,6 +1016,7 @@ 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); static bool published_condition_landed_fw = false; + if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; @@ -994,6 +1030,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { if (!published_condition_landed_fw) { status.condition_landed = false; // Fixedwing does not have a landing detector currently @@ -1063,8 +1100,9 @@ int commander_thread_main(int argc, char *argv[]) /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (last_idle_time > 0) - status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + if (last_idle_time > 0) { + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + } last_idle_time = system_load.tasks[0].total_runtime; @@ -1140,22 +1178,22 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t arming_res; // store all transitions results here /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 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); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1168,7 +1206,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1177,7 +1215,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1190,7 +1228,7 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (res == TRANSITION_CHANGED) { + if (arming_res == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); @@ -1198,24 +1236,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - } else if (res == TRANSITION_DENIED) { + } else if (arming_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status, &sp_man); + transition_result_t main_res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ - if (res == TRANSITION_CHANGED) { + if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); - } else if (res == TRANSITION_DENIED) { + } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } @@ -1228,7 +1266,8 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { - /* MISSION switch */ + + /* LOITER switch */ if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; @@ -1240,7 +1279,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { + pos_sp_triplet.nav_state == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); @@ -1257,34 +1296,39 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed) { if (status.main_state == MAIN_STATE_AUTO) { /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); + + if (auto_res == TRANSITION_NOT_CHANGED) { + last_auto_state_valid = hrt_absolute_time(); + } - if (res == TRANSITION_DENIED) { + /* still invalid state after the timeout interval, execute failsafe */ + if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { + if (auto_res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } else { /* failsafe for manual modes */ - transition_result_t res = TRANSITION_DENIED; + transition_result_t manual_res = TRANSITION_DENIED; if (!status.condition_landed) { /* vehicle is not landed, try to perform RTL */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); } - if (res == TRANSITION_DENIED) { + if (manual_res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { + if (manual_res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } @@ -1292,14 +1336,14 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* reset failsafe when disarmed */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assist switch is on POSCTL position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1313,8 +1357,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) + if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { status_changed = true; + } } /* check which state machines for changes, clear "changed" flag */ @@ -1331,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[]) /* 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_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; @@ -1353,6 +1398,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_home_position_valid = true; } } + was_armed = armed.armed; if (main_state_changed) { @@ -1516,21 +1562,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ - if (leds_counter % 20 == 0) + if (leds_counter % 20 == 0) { led_toggle(LED_BLUE); + } } else { /* not ready to arm, blink at 10Hz */ - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_BLUE); + } } #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); + } } else { led_off(LED_AMBER); @@ -1555,26 +1604,26 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->assisted_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_EASY); + case SWITCH_POS_MIDDLE: // ASSIST + if (sp_man->posctl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTCTL + print_reject_mode(status, "POSCTL"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->assisted_switch != SWITCH_POS_ON) { - print_reject_mode(status, "SEATBELT"); + if (sp_man->posctl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTL"); } // else fallback to MANUAL @@ -1589,9 +1638,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTCTL (POSCTL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1637,7 +1686,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1648,7 +1697,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1744,7 +1793,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: @@ -1794,8 +1843,9 @@ void *commander_low_prio_loop(void *arg) int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); /* timed out - periodic check for thread_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1810,8 +1860,9 @@ void *commander_low_prio_loop(void *arg) if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || cmd.command == VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == VEHICLE_CMD_DO_SET_SERVO) + cmd.command == VEHICLE_CMD_DO_SET_SERVO) { continue; + } /* only handle low-priority commands here */ switch (cmd.command) { @@ -1889,6 +1940,7 @@ void *commander_low_prio_loop(void *arg) /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); + } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { @@ -1903,10 +1955,12 @@ void *commander_low_prio_loop(void *arg) } - if (calib_ret == OK) + if (calib_ret == OK) { tune_positive(true); - else + + } else { tune_negative(true); + } arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); @@ -1926,11 +1980,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1946,11 +2002,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1960,8 +2018,8 @@ void *commander_low_prio_loop(void *arg) } case VEHICLE_CMD_START_RX_PAIR: - /* handled in the IO driver */ - break; + /* handled in the IO driver */ + break; default: /* don't answer on unsupported commands, it will be done in main loop */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0fd3c9e9e..86f77cdb8 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -113,17 +113,22 @@ void buzzer_deinit() close(buzzer); } -void set_tune(int tune) { +void set_tune(int tune) +{ unsigned int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } + tune_current = tune; + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; + } else { tune_end = 0; } @@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); } @@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEUTRAL_TUNE); } @@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEGATIVE_TUNE); } @@ -244,22 +252,25 @@ int led_off(int led) void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + } } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + } } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + } } float battery_remaining_estimate_voltage(float voltage, float discharged) @@ -299,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { /* else use voltage */ ret = remaining_voltage; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a946543d..ee0d08391 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - ut_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTCTL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - ut_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSCTL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; + new_main_state = MAIN_STATE_POSCTL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 30cd0d48d..cbc2844c1 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd) gyro_scale.z_offset += gyro_report.z; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else { poll_errcount++; @@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd) /* apply new offsets */ fd = open(GYRO_DEVICE_PATH, 0); - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) { warn("WARNING: failed to apply new offsets for gyro"); + } close(fd); @@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd) float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; } - if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; + if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; } uint64_t last_time = hrt_absolute_time(); @@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd) //float mag = -atan2f(magNav(1),magNav(0)); float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2 * M_PI_F; + if (mag > M_PI_F) { mag -= 2 * M_PI_F; } - if (mag < -M_PI_F) mag += 2 * M_PI_F; + if (mag < -M_PI_F) { mag += 2 * M_PI_F; } float diff = mag - mag_last; - if (diff > M_PI_F) diff -= 2 * M_PI_F; + if (diff > M_PI_F) { diff -= 2 * M_PI_F; } - if (diff < -M_PI_F) diff += 2 * M_PI_F; + if (diff < -M_PI_F) { diff += 2 * M_PI_F; } baseline_integral += diff; mag_last = mag; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4ebf266f4..9296db6ed 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -124,6 +124,7 @@ int do_mag_calibration(int mavlink_fd) res = ERROR; return res; } + } else { /* exit */ return ERROR; @@ -163,8 +164,9 @@ int do_mag_calibration(int mavlink_fd) calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) + 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++; @@ -198,14 +200,17 @@ int do_mag_calibration(int mavlink_fd) } } - if (x != NULL) + if (x != NULL) { free(x); + } - if (y != NULL) + if (y != NULL) { free(y); + } - if (z != NULL) + if (z != NULL) { free(z); + } if (res == OK) { /* apply calibration and set parameters */ @@ -234,23 +239,29 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { res = ERROR; + } if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 2144d3460..a83c81850 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ 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_ALTCTL, + PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 41f3ca0aa..0776894fb 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; + float p = sp.y; param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; + p = sp.x; param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; + p = sp.r; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f09d586c7..abcf72717 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -75,38 +75,38 @@ static bool failsafe_state_changed = true; // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char* state_names[ARMING_STATE_MAX] = { - "ARMING_STATE_INIT", - "ARMING_STATE_STANDBY", - "ARMING_STATE_ARMED", - "ARMING_STATE_ARMED_ERROR", - "ARMING_STATE_STANDBY_ERROR", - "ARMING_STATE_REBOOT", - "ARMING_STATE_IN_AIR_RESTORE", +static const char *state_names[ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", }; transition_result_t arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none + const struct safety_s *safety, /// current safety settings + arming_state_t new_arming_state, /// arming state requested + struct actuator_armed_s *armed, /// current armed status + const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { - // Double check that our static arrays are still valid - ASSERT(ARMING_STATE_INIT == 0); - ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); - + // Double check that our static arrays are still valid + ASSERT(ARMING_STATE_INIT == 0); + ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + /* * Perform an atomic state update */ @@ -117,63 +117,70 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* only check transition if the new state is actually different from the current one */ if (new_arming_state == status->arming_state) { ret = TRANSITION_NOT_CHANGED; + } else { /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } - - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; - if (valid_transition) { - // We have a good transition. Now perform any secondary validation. - if (new_arming_state == ARMING_STATE_ARMED) { - // Fail transition if we need safety switch press - // Allow if coming from in air restore - // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); - } - valid_transition = false; - } - } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { - new_arming_state = ARMING_STATE_STANDBY_ERROR; - } - } - - // HIL can always go to standby - if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { - valid_transition = true; - } - - /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - valid_transition = false; - } - - // Finish up the state transition - if (valid_transition) { - armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; - ret = TRANSITION_CHANGED; - status->arming_state = new_arming_state; - arming_state_changed = true; - } - } - + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == ARMING_STATE_ARMED) { + // Fail transition if we need safety switch press + // Allow if coming from in air restore + // Allow if HIL_STATE_ON + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } + + valid_transition = false; + } + + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; + } + } + + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; + } + + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) { - static const char* errMsg = "Invalid arming transition from %s to %s"; - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } - warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } + if (ret == TRANSITION_DENIED) { + static const char *errMsg = "Invalid arming transition from %s to %s"; + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } + + warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } return ret; } @@ -215,7 +222,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +233,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || @@ -320,6 +327,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; d = opendir("/dev"); + if (d) { struct dirent *direntry; @@ -331,26 +339,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (!strncmp("tty", direntry->d_name, 3)) { continue; } + /* skip mtd devices */ if (!strncmp("mtd", direntry->d_name, 3)) { continue; } + /* skip ram devices */ if (!strncmp("ram", direntry->d_name, 3)) { continue; } + /* skip MMC devices */ if (!strncmp("mmc", direntry->d_name, 3)) { continue; } + /* skip mavlink */ if (!strcmp("mavlink", direntry->d_name)) { continue; } + /* skip console */ if (!strcmp("console", direntry->d_name)) { continue; } + /* skip null */ if (!strcmp("null", direntry->d_name)) { continue; diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c deleted file mode 100644 index 2aeca3a98..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * - * 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 fixedwing_att_control_rate.c - * Implementation of a fixed wing attitude controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> - -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/systemlib.h> - -#include "fixedwing_att_control_att.h" - - -struct fw_att_control_params { - float roll_p; - float rollrate_lim; - float pitch_p; - float pitchrate_lim; - float yawrate_lim; - float pitch_roll_compensation_p; -}; - -struct fw_pos_control_param_handles { - param_t roll_p; - param_t rollrate_lim; - param_t pitch_p; - param_t pitchrate_lim; - param_t yawrate_lim; - param_t pitch_roll_compensation_p; -}; - - - -/* Internal Prototypes */ -static int parameters_init(struct fw_pos_control_param_handles *h); -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p); - -static int parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->roll_p = param_find("FW_ROLL_P"); - h->rollrate_lim = param_find("FW_ROLLR_LIM"); - h->pitch_p = param_find("FW_PITCH_P"); - h->pitchrate_lim = param_find("FW_PITCHR_LIM"); - h->yawrate_lim = param_find("FW_YAWR_LIM"); - h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP"); - - return OK; -} - -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p) -{ - param_get(h->roll_p, &(p->roll_p)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); - param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); - param_get(h->yawrate_lim, &(p->yawrate_lim)); - param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p)); - - return OK; -} - -int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - const float speed_body[], - struct vehicle_rates_setpoint_s *rates_sp) -{ - static int counter = 0; - static bool initialized = false; - - static struct fw_att_control_params p; - static struct fw_pos_control_param_handles h; - - static PID_t roll_controller; - static PID_t pitch_controller; - - - if (!initialized) { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); - } - - /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); - - - /* Pitch (P) */ - - /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ - float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); - /* set pitch plus feedforward roll compensation */ - rates_sp->pitch = pid_calculate(&pitch_controller, - att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); - - /* Yaw (from coordinated turn constraint or lateral force) */ - rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) - / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); - -// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); - - counter++; - - return 0; -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h b/src/modules/fixedwing_att_control/fixedwing_att_control_att.h deleted file mode 100644 index 600e35b89..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * - * - * 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 Fixed Wing Attitude Control */ - -#ifndef FIXEDWING_ATT_CONTROL_ATT_H_ -#define FIXEDWING_ATT_CONTROL_ATT_H_ - -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_global_position.h> - -int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - const float speed_body[], - struct vehicle_rates_setpoint_s *rates_sp); - -#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c deleted file mode 100644 index b6b4546c2..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ /dev/null @@ -1,367 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Doug Weibel <douglas.weibel@colorado.edu> - * - * 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 fixedwing_att_control.c - * Implementation of a fixed wing attitude controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/debug_key_value.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/perf_counter.h> -#include <systemlib/systemlib.h> - -#include "fixedwing_att_control_rate.h" -#include "fixedwing_att_control_att.h" - -/* Prototypes */ -/** - * Deamon management function. - */ -__EXPORT int fixedwing_att_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_att_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Variables */ -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/* Main Thread */ -int fixedwing_att_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing att control] started\n"); - - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct manual_control_setpoint_s manual_sp; - memset(&manual_sp, 0, sizeof(manual_sp)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); - - /* output structs */ - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - /* subscribe */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* Setup of loop */ - float gyro[3] = {0.0f, 0.0f, 0.0f}; - float speed_body[3] = {0.0f, 0.0f, 0.0f}; - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - - while (!thread_should_exit) { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - /* Check if there is a new position measurement or attitude setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool att_sp_updated; - orb_check(att_sp_sub, &att_sp_updated); - - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (att_sp_updated) - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (att.R_valid) { - speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; - speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; - speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; - - } else { - speed_body[0] = 0; - speed_body[1] = 0; - speed_body[2] = 0; - - printf("FW ATT CONTROL: Did not get a valid R\n"); - } - } - - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); - - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - /* set manual setpoints if required */ - if (control_mode.flag_control_manual_enabled) { - if (control_mode.flag_control_attitude_enabled) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } - - /* execute attitude control if requested */ - if (control_mode.flag_control_attitude_enabled) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } - - /* publish rates */ - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); - - /* sanity check and publish actuator outputs */ - if (isfinite(actuators.control[0]) && - isfinite(actuators.control[1]) && - isfinite(actuators.control[2]) && - isfinite(actuators.control[3])) { - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } - } - - printf("[fixedwing_att_control] exiting, stopping all motors.\n"); - thread_running = false; - - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - - - close(att_sub); - close(actuator_pub); - close(rates_pub); - - fflush(stdout); - exit(0); - - return 0; - -} - -/* Startup Functions */ - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_att_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_att_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_att_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_att_control is running\n"); - - } else { - printf("\tfixedwing_att_control not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c deleted file mode 100644 index cdab39edc..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ /dev/null @@ -1,211 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * - * 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 fixedwing_att_control_rate.c - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * - * Implementation of a fixed wing attitude controller. - * - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> - -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/systemlib.h> - -#include "fixedwing_att_control_rate.h" - -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - -/* feedforward compensation */ -PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ - -struct fw_rate_control_params { - float rollrate_p; - float rollrate_i; - float rollrate_awu; - float pitchrate_p; - float pitchrate_i; - float pitchrate_awu; - float yawrate_p; - float yawrate_i; - float yawrate_awu; - float pitch_thr_ff; -}; - -struct fw_rate_control_param_handles { - param_t rollrate_p; - param_t rollrate_i; - param_t rollrate_awu; - param_t pitchrate_p; - param_t pitchrate_i; - param_t pitchrate_awu; - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_awu; - param_t pitch_thr_ff; -}; - - - -/* Internal Prototypes */ -static int parameters_init(struct fw_rate_control_param_handles *h); -static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p); - -static int parameters_init(struct fw_rate_control_param_handles *h) -{ - /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); - - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); - h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); - h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); - - return OK; -} - -static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p) -{ - param_get(h->rollrate_p, &(p->rollrate_p)); - param_get(h->rollrate_i, &(p->rollrate_i)); - param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->pitchrate_p, &(p->pitchrate_p)); - param_get(h->pitchrate_i, &(p->pitchrate_i)); - param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_awu, &(p->yawrate_awu)); - param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); - - return OK; -} - -int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators) -{ - static int counter = 0; - static bool initialized = false; - - static struct fw_rate_control_params p; - static struct fw_rate_control_param_handles h; - - static PID_t roll_rate_controller; - static PID_t pitch_rate_controller; - static PID_t yaw_rate_controller; - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if (!initialized) { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); - } - - - /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - counter++; - - return 0; -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h deleted file mode 100644 index 500e3e197..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h +++ /dev/null @@ -1,48 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * - * - * 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 Fixed Wing Attitude Rate Control */ - -#ifndef FIXEDWING_ATT_CONTROL_RATE_H_ -#define FIXEDWING_ATT_CONTROL_RATE_H_ - -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/actuator_controls.h> - -int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators); - -#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */ diff --git a/src/modules/fixedwing_att_control/module.mk b/src/modules/fixedwing_att_control/module.mk deleted file mode 100644 index fd1a8724a..000000000 --- a/src/modules/fixedwing_att_control/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Fixedwing Attitude Control application -# - -MODULE_COMMAND = fixedwing_att_control - -SRCS = fixedwing_att_control_main.c \ - fixedwing_att_control_att.c \ - fixedwing_att_control_rate.c diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae07275..bbb39f20f 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c deleted file mode 100644 index 888dd0942..000000000 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ /dev/null @@ -1,479 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Doug Weibel <douglas.weibel@colorado.edu> - * - * 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 fixedwing_pos_control.c - * Implementation of a fixed wing attitude controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/parameter_update.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/perf_counter.h> -#include <systemlib/systemlib.h> - -/* - * Controller parameters, accessible via MAVLink - * - */ -PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value -PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track -PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians -PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */ - -struct fw_pos_control_params { - float heading_p; - float headingr_p; - float headingr_i; - float headingr_lim; - float xtrack_p; - float altitude_p; - float roll_lim; - float pitch_lim; -}; - -struct fw_pos_control_param_handles { - param_t heading_p; - param_t headingr_p; - param_t headingr_i; - param_t headingr_lim; - param_t xtrack_p; - param_t altitude_p; - param_t roll_lim; - param_t pitch_lim; -}; - - -struct planned_path_segments_s { - bool segment_type; - double start_lat; // Start of line or center of arc - double start_lon; - double end_lat; - double end_lon; - float radius; // Radius of arc - float arc_start_bearing; // Bearing from center to start of arc - float arc_sweep; // Angle (radians) swept out by arc around center. - // Positive for clockwise, negative for counter-clockwise -}; - - -/* Prototypes */ -/* Internal Prototypes */ -static int parameters_init(struct fw_pos_control_param_handles *h); -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); - -/** - * Deamon management function. - */ -__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Variables */ -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - - -/** - * Parameter management - */ -static int parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->heading_p = param_find("FW_HEAD_P"); - h->headingr_p = param_find("FW_HEADR_P"); - h->headingr_i = param_find("FW_HEADR_I"); - h->headingr_lim = param_find("FW_HEADR_LIM"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - - return OK; -} - -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) -{ - param_get(h->heading_p, &(p->heading_p)); - param_get(h->headingr_p, &(p->headingr_p)); - param_get(h->headingr_i, &(p->headingr_i)); - param_get(h->headingr_lim, &(p->headingr_lim)); - param_get(h->xtrack_p, &(p->xtrack_p)); - param_get(h->altitude_p, &(p->altitude_p)); - param_get(h->roll_lim, &(p->roll_lim)); - param_get(h->pitch_lim, &(p->pitch_lim)); - - return OK; -} - - -/* Main Thread */ -int fixedwing_pos_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing pos control] started\n"); - - /* declare and safely initialize all structs */ - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct vehicle_global_position_s start_pos; // Temporary variable, replace with - memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available - struct vehicle_global_position_setpoint_s global_setpoint; - memset(&global_setpoint, 0, sizeof(global_setpoint)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct crosstrack_error_s xtrack_err; - memset(&xtrack_err, 0, sizeof(xtrack_err)); - struct parameter_update_s param_update; - memset(¶m_update, 0, sizeof(param_update)); - - /* output structs */ - struct vehicle_attitude_setpoint_s attitude_setpoint; - memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); - - /* publish attitude setpoint */ - attitude_setpoint.roll_body = 0.0f; - attitude_setpoint.pitch_body = 0.0f; - attitude_setpoint.yaw_body = 0.0f; - attitude_setpoint.thrust = 0.0f; - orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); - - /* subscribe */ - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - - /* Setup of loop */ - struct pollfd fds[2] = { - { .fd = param_sub, .events = POLLIN }, - { .fd = att_sub, .events = POLLIN } - }; - bool global_sp_updated_set_once = false; - - float psi_track = 0.0f; - - int counter = 0; - - struct fw_pos_control_params p; - struct fw_pos_control_param_handles h; - - PID_t heading_controller; - PID_t heading_rate_controller; - PID_t offtrack_controller; - PID_t altitude_controller; - - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value - - /* error and performance monitoring */ - perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); - perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); - - while (!thread_should_exit) { - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(fw_err_perf); - - } else if (ret == 0) { - /* no return value, ignore */ - } else { - - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); - - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value - } - - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { - - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - } - - if (global_sp_updated) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - printf("next wp direction: %0.4f\n", (double)psi_track); - } - - /* Simple Horizontal Control */ - if (global_sp_updated_set_once) { - // if (counter % 100 == 0) - // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - // XXX what is xtrack_err.past_end? - if (distance_res == OK /*&& !xtrack_err.past_end*/) { - - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance - - float psi_c = psi_track + delta_psi_c; - float psi_e = psi_c - att.yaw; - - /* wrap difference back onto -pi..pi range */ - psi_e = _wrap_pi(psi_e); - - if (verbose) { - printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); - printf("delta_psi_c %.4f ", (double)delta_psi_c); - printf("psi_c %.4f ", (double)psi_c); - printf("att.yaw %.4f ", (double)att.yaw); - printf("psi_e %.4f ", (double)psi_e); - } - - /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following - float psi_rate_c = delta_psi_rate_c + psi_rate_track; - - /* limit turn rate */ - if (psi_rate_c > p.headingr_lim) { - psi_rate_c = p.headingr_lim; - - } else if (psi_rate_c < -p.headingr_lim) { - psi_rate_c = -p.headingr_lim; - } - - float psi_rate_e = psi_rate_c - att.yawspeed; - - // XXX sanity check: Assume 10 m/s stall speed and no stall condition - float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - - if (ground_speed < 10.0f) { - ground_speed = 10.0f; - } - - float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); - - if (verbose) { - printf("psi_rate_c %.4f ", (double)psi_rate_c); - printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); - printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); - } - - if (verbose && counter % 100 == 0) - printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c); - - } else { - if (verbose && counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); - } - - /* Very simple Altitude Control */ - if (pos_updated) { - - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); - - } - - // XXX need speed control - attitude_setpoint.thrust = 0.7f; - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - - /* measure in what intervals the controller runs */ - perf_count(fw_interval_perf); - - counter++; - - } else { - // XXX no setpoint, decent default needed (loiter?) - } - } - } - } - - printf("[fixedwing_pos_control] exiting.\n"); - thread_running = false; - - - close(attitude_setpoint_pub); - - fflush(stdout); - exit(0); - - return 0; - -} - -/* Startup Functions */ - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_pos_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_pos_control is running\n"); - - } else { - printf("\tfixedwing_pos_control not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/src/modules/fixedwing_pos_control/module.mk b/src/modules/fixedwing_pos_control/module.mk deleted file mode 100644 index b976377e9..000000000 --- a/src/modules/fixedwing_pos_control/module.mk +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Fixedwing PositionControl application -# - -MODULE_COMMAND = fixedwing_pos_control - -SRCS = fixedwing_pos_control_main.c diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 5276b1c13..1c411fa06 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -273,7 +273,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace att_control @@ -706,20 +706,21 @@ FixedwingAttitudeControl::task_main() } else { /* * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do 45 degrees, the mapping is - * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. If more than 45 degrees are desired, - * a scaling parameter can be applied to the remote. + * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - throttle_sp = _manual.throttle; + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* @@ -825,10 +826,10 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.roll; - _actuators.control[1] = -_manual.pitch; - _actuators.control[2] = _manual.yaw; - _actuators.control[3] = _manual.throttle; + _actuators.control[0] = _manual.y; + _actuators.control[1] = -_manual.x; + _actuators.control[2] = _manual.r; + _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index f076c94fd..fb8abe95a 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -237,7 +237,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace estimator diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5cdab10a1..20f3fefcd 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -89,6 +89,7 @@ #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> #include <external_lgpl/tecs/tecs.h> +#include <drivers/drv_range_finder.h> #include "landingslope.h" #include "mtecs/mTecs.h" @@ -135,6 +136,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ + int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -148,13 +150,14 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -182,7 +185,7 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_last; + float flare_curve_alt_rel_last; /* heading hold */ float target_bearing; @@ -242,6 +245,7 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -286,6 +290,7 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -312,6 +317,12 @@ private: bool vehicle_airspeed_poll(); /** + * Check for range finder updates. + */ + bool range_finder_poll(); + + + /** * Check for position updates. */ void vehicle_attitude_poll(); @@ -332,6 +343,11 @@ private: void navigation_capabilities_publish(); /** + * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + */ + float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + + /** * Control position. */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, @@ -348,7 +364,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); /* * Reset takeoff state @@ -368,6 +384,7 @@ private: float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, bool climbout_mode, float climbout_pitch_min_rad, + float altitude, const math::Vector<3> &ground_speed, fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL); @@ -399,6 +416,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -418,7 +436,7 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), last_manual(false), usePreTakeoffThrust(false), - flare_curve_alt_last(0.0f), + flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), @@ -434,7 +452,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _pos_sp_triplet(), _sensor_combined(), _mTecs(), - _was_pos_control_mode(false) + _was_pos_control_mode(false), + _range_finder() { _nav_capabilities.turn_distance = 0.0f; @@ -459,6 +478,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -548,6 +568,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -646,6 +668,20 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::range_finder_poll() +{ + /* check if there is a range finder measurement */ + bool range_finder_updated; + orb_check(_range_finder_sub, &range_finder_updated); + + if (range_finder_updated) { + orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); + } + + return range_finder_updated; +} + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -774,6 +810,23 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } +float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +{ + float rel_alt_estimated = current_alt - land_setpoint_alt; + + /* only use range finder if: + * parameter (range_finder_use_relative_alt) > 0 + * the measurement is valid + * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt + */ + if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + return rel_alt_estimated; + } + + return range_finder.distance; + +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) @@ -852,7 +905,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), ground_speed); + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { @@ -865,7 +918,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), ground_speed); + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -924,12 +977,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + + float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -939,7 +994,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -948,19 +1003,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { - flare_curve_alt = pos_sp_triplet.current.alt; + flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas, + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + calculate_target_airspeed(airspeed_land), eas2tas, flare_pitch_angle_rad, math::radians(15.0f), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad, ground_speed, land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND); + false, flare_pitch_angle_rad, + _pos_sp_triplet.current.alt + relative_alt, ground_speed, + land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -968,7 +1026,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - flare_curve_alt_last = flare_curve_alt; + flare_curve_alt_rel_last = flare_curve_alt_rel; } else { /* intersect glide slope: @@ -976,22 +1034,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ - float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + float altitude_desired_rel = relative_alt; + if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { /* stay on slope */ - altitude_desired = landing_slope_alt_desired; + altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } } else { /* continue horizontally */ - altitude_desired = math::max(_global_pos.alt, L_altitude); + altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas, + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _pos_sp_triplet.current.alt + relative_alt, false, math::radians(_parameters.pitch_limit_min), ground_speed); } @@ -1030,10 +1089,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_cruise, + true, + math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + fwPosctrl::mTecs::TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); @@ -1042,7 +1110,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), ground_speed); + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } } else { @@ -1074,23 +1142,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* posctrl mode enabled */) { _was_pos_control_mode = false; - /** EASY FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; - } + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; + } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; - } + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; + } - //XXX not used + //XXX not used - /* climb out control */ + /* climb out control */ // bool climb_out = false; // // /* user wants to climb out */ @@ -1098,44 +1166,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ - // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + // XXX check if ground speed undershoot should be applied here + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), ground_speed); + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* altctrl mode enabled */) { _was_pos_control_mode = false; - /** SEATBELT FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; /* user switched off throttle */ - if (_manual.throttle < 0.1f) { + if (_manual.z < 0.1f) { throttle_max = 0.0f; /* switch to pure pitch based altitude control, give up speed */ _tecs.set_speed_weight(0.0f); @@ -1145,17 +1214,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climb_out = false; /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + if (_manual.x > 0.3f && _manual.z > 0.8f) { climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d); - _att_sp.roll_body = _manual.roll; - _att_sp.yaw_body = _manual.yaw; - tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); + _att_sp.roll_body = _manual.y; + _att_sp.yaw_body = _manual.r; + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - climb_out, math::radians(_parameters.pitch_limit_min), ground_speed); + climb_out, math::radians(_parameters.pitch_limit_min), + _global_pos.alt, ground_speed); } else { @@ -1210,6 +1280,7 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1289,6 +1360,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + range_finder_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); @@ -1357,6 +1429,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, bool climbout_mode, float climbout_pitch_min_rad, + float altitude, const math::Vector<3> &ground_speed, fwPosctrl::mTecs::tecs_mode mode) { @@ -1366,9 +1439,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode); + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode); } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 37f06dbe5..f258f77da 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -375,3 +375,14 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); + +/** + * Relative altitude threshold for range finder measurements for use during landing + * + * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT + * set to < 0 to disable + * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index e5f7023ae..8ce465fe8 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues() _horizontal_slope_displacement = (_flare_length - _d1); } -float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { - return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); + return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + return getLandingSlopeRelativeAltitude(wp_landing_distance); + else + return 0.0f; +} + +float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude) +{ + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); +} + +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +{ + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude); else return wp_altitude; } -float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; else - return wp_landing_altitude; + return 0.0f; +} + +float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + + return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 76d65a55f..b54fd501c 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -63,11 +63,26 @@ public: Landingslope() {} ~Landingslope() {} + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeRelativeAltitude(float wp_landing_distance); + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); + + /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude); /** * @@ -78,11 +93,20 @@ public: /** * + * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + __EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative + } + + /** + * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) { - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude; } /** @@ -95,8 +119,9 @@ public: } + float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); - float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); + float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7ecca0d65..dd88b0949 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -727,9 +727,9 @@ int Mavlink::mavlink_pm_send_param(param_t param) if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ - static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; + char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; float val_buf; - static mavlink_message_t tx_msg; + mavlink_message_t tx_msg; /* query parameter type */ param_type_t type = param_type(param); @@ -1535,6 +1535,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; + statustext.severity = MAV_SEVERITY_INFO; + int i = 0; while (i < len - 1) { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce1645..79dd88657 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_STATE_POSCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -819,11 +819,11 @@ protected: void send(const hrt_abstime t) { - bool updated = status_sub->update(t); - updated |= pos_sp_triplet_sub->update(t); - updated |= act_sub->update(t); + bool updated = act_sub->update(t); + (void)pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); - if (updated) { + if (updated && (status->arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -1138,10 +1138,10 @@ protected: if (manual_sub->update(t)) { mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, + manual->x * 1000, + manual->y * 1000, + manual->z * 1000, + manual->r * 1000, 0); } } @@ -1339,22 +1339,23 @@ protected: void send(const hrt_abstime t) { - (void)range_sub->update(t); + if (range_sub->update(t)) { - uint8_t type; + uint8_t type; - switch (range->type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, - range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c93c1c00..b03a68c07 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -191,7 +191,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); @@ -217,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) _mavlink->_task_should_exit = true; } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP ID"); + return; + } + struct vehicle_command_s vcmd; memset(&vcmd, 0, sizeof(vcmd)); @@ -432,10 +438,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) memset(&manual, 0, sizeof(manual)); manual.timestamp = hrt_absolute_time(); - manual.pitch = man.x / 1000.0f; - manual.roll = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 36d95bf06..a69153bf0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -71,7 +71,7 @@ #include <systemlib/err.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> -#include <mathlib/mathlib.h> +#include <lib/mathlib/mathlib.h> #include <lib/geo/geo.h> /** @@ -156,6 +156,7 @@ private: param_t yaw_rate_i; param_t yaw_rate_d; param_t yaw_ff; + param_t yaw_rate_max; param_t man_roll_max; param_t man_pitch_max; @@ -168,6 +169,7 @@ private: math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ float man_roll_max; float man_pitch_max; @@ -225,9 +227,9 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main attitude control task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace mc_att_control @@ -276,6 +278,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; _rates_prev.zero(); _rates_sp.zero(); @@ -298,7 +305,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - + _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); @@ -367,15 +374,16 @@ MulticopterAttitudeControl::parameters_update() _params.rate_d(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); + param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); /* manual control scale */ param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - - _params.man_roll_max *= M_PI / 180.0; - _params.man_pitch_max *= M_PI / 180.0; - _params.man_yaw_max *= M_PI / 180.0; + _params.man_roll_max = math::radians(_params.man_roll_max); + _params.man_pitch_max = math::radians(_params.man_pitch_max); + _params.man_yaw_max = math::radians(_params.man_yaw_max); return OK; } @@ -478,7 +486,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.throttle; + _v_att_sp.thrust = _manual_control_sp.z; publish_att_sp = true; } @@ -496,7 +504,7 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max; + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -512,8 +520,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max; + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; publish_att_sp = true; } @@ -626,6 +634,9 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); + /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index e52c39368..19134c7b4 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -175,6 +175,18 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); /** + * Max yaw rate + * + * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); + +/** * Max manual roll * * @unit deg diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7c625a0c5..6e611d4ab 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -226,7 +226,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace pos_control @@ -617,7 +617,7 @@ MulticopterPositionControl::task_main() reset_alt_sp(); /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { @@ -625,8 +625,8 @@ MulticopterPositionControl::task_main() reset_pos_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.pitch; - sp_move_rate(1) = _manual.roll; + sp_move_rate(0) = _manual.x; + sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ @@ -782,7 +782,7 @@ MulticopterPositionControl::task_main() float i = _params.thr_min; if (reset_int_z_manual) { - i = _manual.throttle; + i = _manual.z; if (i < _params.thr_min) { i = _params.thr_min; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 104df4e59..dacdd46f0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217d..646ab6ab9 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -100,8 +100,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss /* Check if all mission items are inside the geofence (if we have a valid geofence) */ if (geofence.valid()) { for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -125,8 +125,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 12fd35a0a..1a45e1345 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1291,7 +1291,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; @@ -1351,7 +1351,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; diff --git a/src/modules/position_estimator/module.mk b/src/modules/position_estimator/module.mk deleted file mode 100644 index f64095d9d..000000000 --- a/src/modules/position_estimator/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the position estimator -# - -MODULE_COMMAND = position_estimator - -# XXX this should be converted to a deamon, its a pretty bad example app -MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT -MODULE_STACKSIZE = 4096 - -SRCS = position_estimator_main.c diff --git a/src/modules/position_estimator/position_estimator_main.c b/src/modules/position_estimator/position_estimator_main.c deleted file mode 100644 index e84945299..000000000 --- a/src/modules/position_estimator/position_estimator_main.c +++ /dev/null @@ -1,423 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * 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 position_estimator_main.c - * Model-identification based position estimator for multirotors - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdlib.h> -#include <stdio.h> -#include <stdbool.h> -#include <fcntl.h> -#include <float.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <limits.h> -#include <math.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <poll.h> - -#define N_STATES 6 -#define ERROR_COVARIANCE_INIT 3 -#define R_EARTH 6371000.0 - -#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 -#define REPROJECTION_COUNTER_LIMIT 125 - -__EXPORT int position_estimator_main(int argc, char *argv[]); - -static uint16_t position_estimator_counter_position_information; - -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -/** - * Initializes the map transformation. - * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 -{ - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; - -} - -/** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_project(double lat, double lon, float *x, float *y) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; - - double sin_phi = sin(phi); - double cos_phi = cos(phi); - - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); - - if (0 != c) - k_bar = c / sin(c); - - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; - -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); -} - -/** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system - * - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_reproject(float x, float y, double *lat, double *lon) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); - double sin_c = sin(c); - double cos_c = cos(c); - - double lat_sphere = 0; - - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); - - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); - - } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } - } - -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - -} - -/**************************************************************************** - * main - ****************************************************************************/ - -int position_estimator_main(int argc, char *argv[]) -{ - - /* welcome user */ - printf("[multirotor position_estimator] started\n"); - - /* initialize values */ - static float u[2] = {0, 0}; - static float z[3] = {0, 0, 0}; - static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0}; - static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0 - }; - - static float xapo1[N_STATES]; - static float Papo1[36]; - - static float gps_covariance[3] = {0.0f, 0.0f, 0.0f}; - - static uint16_t counter = 0; - position_estimator_counter_position_information = 0; - - uint8_t predict_only = 1; - - bool gps_valid = false; - - bool new_initialization = true; - - static double lat_current = 0.0d;//[°]] --> 47.0 - static double lon_current = 0.0d; //[°]] -->8.5 - float alt_current = 0.0f; - - - //TODO: handle flight without gps but with estimator - - /* subscribe to vehicle status, attitude, gps */ - struct vehicle_gps_position_s gps; - gps.fix_type = 0; - struct vehicle_status_s vstatus; - struct vehicle_attitude_s att; - - int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* subscribe to attitude at 100 Hz */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - /* wait until gps signal turns valid, only then can we initialize the projection */ - while (gps.fix_type < 3) { - struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds, 1, 5000)) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - /* Read wether the vehicle status changed */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - gps_valid = (gps.fix_type > 2); - } - } - - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7; - lon_current = ((double)(gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - - /* publish global position messages only after first GPS message */ - struct vehicle_local_position_s local_pos = { - .x = 0, - .y = 0, - .z = 0 - }; - orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - - printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - - while (1) { - - /*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */ - struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} }; - - if (poll(fds, 1, 5000) <= 0) { - /* error / timeout */ - } else { - - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - /* got attitude, updating pos as well */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); - - /*copy attitude */ - u[0] = att.roll; - u[1] = att.pitch; - - /* initialize map projection with the last estimate (not at full rate) */ - if (gps.fix_type > 2) { - /* Project gps lat lon (Geographic coordinate system) to plane*/ - map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1])); - - local_pos.x = z[0]; - local_pos.y = z[1]; - /* negative offset from initialization altitude */ - local_pos.z = alt_current - (gps.alt) * 1e-3; - - - orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); - } - - - // gps_covariance[0] = gps.eph; //TODO: needs scaling - // gps_covariance[1] = gps.eph; - // gps_covariance[2] = gps.epv; - - // } else { - // /* we can not use the gps signal (it is of low quality) */ - // predict_only = 1; - // } - - // // predict_only = 0; //TODO: only for testing, removeme, XXX - // // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX - // // usleep(100000); //TODO: only for testing, removeme, XXX - - - // /*Get new estimation (this is calculated in the plane) */ - // //TODO: if new_initialization == true: use 0,0,0, else use xapo - // if (true == new_initialization) { //TODO,XXX: uncomment! - // xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane - // xapo[2] = 0; - // xapo[4] = 0; - // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - - // } else { - // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - // } - - - - // /* Copy values from xapo1 to xapo */ - // int i; - - // for (i = 0; i < N_STATES; i++) { - // xapo[i] = xapo1[i]; - // } - - // if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { - // /* Reproject from plane to geographic coordinate system */ - // // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! - // map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme - // // //DEBUG - // // if(counter%500 == 0) - // // { - // // printf("phi_1: %.10f\n", phi_1); - // // printf("lambda_0: %.10f\n", lambda_0); - // // printf("lat_estimated: %.10f\n", lat_current); - // // printf("lon_estimated: %.10f\n", lon_current); - // // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); - // // fflush(stdout); - // // - // // } - - // // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) - // // { - // /* send out */ - - // global_pos.lat = lat_current; - // global_pos.lon = lon_current; - // global_pos.alt = xapo1[4]; - // global_pos.vx = xapo1[1]; - // global_pos.vy = xapo1[3]; - // global_pos.vz = xapo1[5]; - - /* publish current estimate */ - // orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); - // } - // else - // { - // printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]); - // fflush(stdout); - // } - - // } - - counter++; - } - - } - - return 0; -} - - diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c deleted file mode 100755 index 977565b8e..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c +++ /dev/null @@ -1,58 +0,0 @@ -/* - * kalman_dlqe1.c - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], - const real32_T x_aposteriori_k[3], real32_T z, real32_T - x_aposteriori[3]) -{ - printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z); - printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2])); - real32_T y; - int32_T i0; - real32_T b_y[3]; - int32_T i1; - real32_T f0; - y = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - b_y[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_y[i0] += C[i1] * A[i1 + 3 * i0]; - } - - y += b_y[i0] * x_aposteriori_k[i0]; - } - - y = z - y; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - - x_aposteriori[i0] = f0 + K[i0] * y; - } - //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2])); -} - -/* End of code generation (kalman_dlqe1.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h deleted file mode 100755 index 2f5330563..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1.h - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -#ifndef __KALMAN_DLQE1_H__ -#define __KALMAN_DLQE1_H__ -/* Include files */ -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); -#endif -/* End of code generation (kalman_dlqe1.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c deleted file mode 100755 index 6627f76e9..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe1_initialize.c - * - * Code generation for function 'kalman_dlqe1_initialize' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" -#include "kalman_dlqe1_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (kalman_dlqe1_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h deleted file mode 100755 index a77eb5712..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1_initialize.h - * - * Code generation for function 'kalman_dlqe1_initialize' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -#ifndef __KALMAN_DLQE1_INITIALIZE_H__ -#define __KALMAN_DLQE1_INITIALIZE_H__ -/* Include files */ -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1_initialize(void); -#endif -/* End of code generation (kalman_dlqe1_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c deleted file mode 100755 index a65536f79..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe1_terminate.c - * - * Code generation for function 'kalman_dlqe1_terminate' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" -#include "kalman_dlqe1_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (kalman_dlqe1_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h deleted file mode 100755 index 100c7f76c..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1_terminate.h - * - * Code generation for function 'kalman_dlqe1_terminate' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -#ifndef __KALMAN_DLQE1_TERMINATE_H__ -#define __KALMAN_DLQE1_TERMINATE_H__ -/* Include files */ -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1_terminate(void); -#endif -/* End of code generation (kalman_dlqe1_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h deleted file mode 100755 index d4b2c2d61..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * kalman_dlqe1_types.h - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -#ifndef __KALMAN_DLQE1_TYPES_H__ -#define __KALMAN_DLQE1_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (kalman_dlqe1_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c deleted file mode 100755 index 11b999064..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c +++ /dev/null @@ -1,119 +0,0 @@ -/* - * kalman_dlqe2.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T f1; - real32_T f2; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else { - f1 = (real32_T)fabs(u0); - f2 = (real32_T)fabs(u1); - if (rtIsInfF(u1)) { - if (f1 == 1.0F) { - y = ((real32_T)rtNaN); - } else if (f1 > 1.0F) { - if (u1 > 0.0F) { - y = ((real32_T)rtInf); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = ((real32_T)rtInf); - } - } else if (f2 == 0.0F) { - y = 1.0F; - } else if (f2 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } else if (u1 == 2.0F) { - y = u0 * u0; - } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrt(u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { - y = ((real32_T)rtNaN); - } else { - y = (real32_T)pow(u0, u1); - } - } - - return y; -} - -void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const - real32_T x_aposteriori_k[3], real32_T z, real32_T - x_aposteriori[3]) -{ - //printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3)); - //printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3)); - real32_T A[9]; - real32_T y; - int32_T i0; - static const int8_T iv0[3] = { 0, 0, 1 }; - - real32_T b_k1[3]; - int32_T i1; - static const int8_T iv1[3] = { 1, 0, 0 }; - - real32_T f0; - A[0] = 1.0F; - A[3] = dt; - A[6] = 0.5F * rt_powf_snf(dt, 2.0F); - A[1] = 0.0F; - A[4] = 1.0F; - A[7] = dt; - y = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - A[2 + 3 * i0] = (real32_T)iv0[i0]; - b_k1[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; - } - - y += b_k1[i0] * x_aposteriori_k[i0]; - } - - y = z - y; - b_k1[0] = k1; - b_k1[1] = k2; - b_k1[2] = k3; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - - x_aposteriori[i0] = f0 + b_k1[i0] * y; - } -} - -/* End of code generation (kalman_dlqe2.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h deleted file mode 100755 index 30170ae22..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __KALMAN_DLQE2_H__ -#define __KALMAN_DLQE2_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); -#endif -/* End of code generation (kalman_dlqe2.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c deleted file mode 100755 index de5a1d8aa..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe2_initialize.c - * - * Code generation for function 'kalman_dlqe2_initialize' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" -#include "kalman_dlqe2_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe2_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (kalman_dlqe2_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h deleted file mode 100755 index 3d507ff31..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2_initialize.h - * - * Code generation for function 'kalman_dlqe2_initialize' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_INITIALIZE_H__ -#define __KALMAN_DLQE2_INITIALIZE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2_initialize(void); -#endif -/* End of code generation (kalman_dlqe2_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c deleted file mode 100755 index 0757c878c..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe2_terminate.c - * - * Code generation for function 'kalman_dlqe2_terminate' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" -#include "kalman_dlqe2_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe2_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (kalman_dlqe2_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h deleted file mode 100755 index 23995020b..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2_terminate.h - * - * Code generation for function 'kalman_dlqe2_terminate' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_TERMINATE_H__ -#define __KALMAN_DLQE2_TERMINATE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2_terminate(void); -#endif -/* End of code generation (kalman_dlqe2_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h deleted file mode 100755 index f7a04d908..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * kalman_dlqe2_types.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_TYPES_H__ -#define __KALMAN_DLQE2_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (kalman_dlqe2_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c deleted file mode 100755 index 9efe2ea7a..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c +++ /dev/null @@ -1,137 +0,0 @@ -/*
- * kalman_dlqe3.c
- *
- * Code generation for function 'kalman_dlqe3'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "randn.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1);
-
-/* Function Definitions */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1)
-{
- real32_T y;
- real32_T f1;
- real32_T f2;
- if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
- y = ((real32_T)rtNaN);
- } else {
- f1 = (real32_T)fabs(u0);
- f2 = (real32_T)fabs(u1);
- if (rtIsInfF(u1)) {
- if (f1 == 1.0F) {
- y = ((real32_T)rtNaN);
- } else if (f1 > 1.0F) {
- if (u1 > 0.0F) {
- y = ((real32_T)rtInf);
- } else {
- y = 0.0F;
- }
- } else if (u1 > 0.0F) {
- y = 0.0F;
- } else {
- y = ((real32_T)rtInf);
- }
- } else if (f2 == 0.0F) {
- y = 1.0F;
- } else if (f2 == 1.0F) {
- if (u1 > 0.0F) {
- y = u0;
- } else {
- y = 1.0F / u0;
- }
- } else if (u1 == 2.0F) {
- y = u0 * u0;
- } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
- y = (real32_T)sqrt(u0);
- } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
- y = ((real32_T)rtNaN);
- } else {
- y = (real32_T)pow(u0, u1);
- }
- }
-
- return y;
-}
-
-void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
- real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,
- real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])
-{
- real32_T A[9];
- int32_T i0;
- static const int8_T iv0[3] = { 0, 0, 1 };
-
- real_T b;
- real32_T y;
- real32_T b_y[3];
- int32_T i1;
- static const int8_T iv1[3] = { 1, 0, 0 };
-
- real32_T b_k1[3];
- real32_T f0;
- A[0] = 1.0F;
- A[3] = dt;
- A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
- A[1] = 0.0F;
- A[4] = 1.0F;
- A[7] = dt;
- for (i0 = 0; i0 < 3; i0++) {
- A[2 + 3 * i0] = (real32_T)iv0[i0];
- }
-
- if (addNoise == 1.0F) {
- b = randn();
- z += sigma * (real32_T)b;
- }
-
- if (posUpdate != 0.0F) {
- y = 0.0F;
- for (i0 = 0; i0 < 3; i0++) {
- b_y[i0] = 0.0F;
- for (i1 = 0; i1 < 3; i1++) {
- b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
- }
-
- y += b_y[i0] * x_aposteriori_k[i0];
- }
-
- y = z - y;
- b_k1[0] = k1;
- b_k1[1] = k2;
- b_k1[2] = k3;
- for (i0 = 0; i0 < 3; i0++) {
- f0 = 0.0F;
- for (i1 = 0; i1 < 3; i1++) {
- f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
- }
-
- x_aposteriori[i0] = f0 + b_k1[i0] * y;
- }
- } else {
- for (i0 = 0; i0 < 3; i0++) {
- x_aposteriori[i0] = 0.0F;
- for (i1 = 0; i1 < 3; i1++) {
- x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];
- }
- }
- }
-}
-
-/* End of code generation (kalman_dlqe3.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h deleted file mode 100755 index 9bbffbbb3..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * kalman_dlqe3.h
- *
- * Code generation for function 'kalman_dlqe3'
- *
- * C source code generated on: Tue Feb 19 15:26:32 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_H__
-#define __KALMAN_DLQE3_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);
-#endif
-/* End of code generation (kalman_dlqe3.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c deleted file mode 100755 index 8f2275c13..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c +++ /dev/null @@ -1,32 +0,0 @@ -/*
- * kalman_dlqe3_data.c
- *
- * Code generation for function 'kalman_dlqe3_data'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "kalman_dlqe3_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-uint32_T method;
-uint32_T state[2];
-uint32_T b_method;
-uint32_T b_state;
-uint32_T c_state[2];
-boolean_T state_not_empty;
-
-/* Function Declarations */
-
-/* Function Definitions */
-/* End of code generation (kalman_dlqe3_data.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h deleted file mode 100755 index 952eb7b89..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h +++ /dev/null @@ -1,38 +0,0 @@ -/*
- * kalman_dlqe3_data.h
- *
- * Code generation for function 'kalman_dlqe3_data'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_DATA_H__
-#define __KALMAN_DLQE3_DATA_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-extern uint32_T method;
-extern uint32_T state[2];
-extern uint32_T b_method;
-extern uint32_T b_state;
-extern uint32_T c_state[2];
-extern boolean_T state_not_empty;
-
-/* Variable Definitions */
-
-/* Function Declarations */
-#endif
-/* End of code generation (kalman_dlqe3_data.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c deleted file mode 100755 index b87d604c4..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c +++ /dev/null @@ -1,47 +0,0 @@ -/*
- * kalman_dlqe3_initialize.c
- *
- * Code generation for function 'kalman_dlqe3_initialize'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "kalman_dlqe3_initialize.h"
-#include "kalman_dlqe3_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe3_initialize(void)
-{
- int32_T i;
- static const uint32_T uv0[2] = { 362436069U, 0U };
-
- rt_InitInfAndNaN(8U);
- state_not_empty = FALSE;
- b_state = 1144108930U;
- b_method = 7U;
- method = 0U;
- for (i = 0; i < 2; i++) {
- c_state[i] = 362436069U + 158852560U * (uint32_T)i;
- state[i] = uv0[i];
- }
-
- if (state[1] == 0U) {
- state[1] = 521288629U;
- }
-}
-
-/* End of code generation (kalman_dlqe3_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h deleted file mode 100755 index 9dee90f9e..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * kalman_dlqe3_initialize.h
- *
- * Code generation for function 'kalman_dlqe3_initialize'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_INITIALIZE_H__
-#define __KALMAN_DLQE3_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe3_initialize(void);
-#endif
-/* End of code generation (kalman_dlqe3_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c deleted file mode 100755 index b00858205..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/*
- * kalman_dlqe3_terminate.c
- *
- * Code generation for function 'kalman_dlqe3_terminate'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "kalman_dlqe3_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe3_terminate(void)
-{
- /* (no terminate code required) */
-}
-
-/* End of code generation (kalman_dlqe3_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h deleted file mode 100755 index 69cc85c76..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * kalman_dlqe3_terminate.h
- *
- * Code generation for function 'kalman_dlqe3_terminate'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_TERMINATE_H__
-#define __KALMAN_DLQE3_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe3_terminate(void);
-#endif
-/* End of code generation (kalman_dlqe3_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h deleted file mode 100755 index f36ea4557..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/*
- * kalman_dlqe3_types.h
- *
- * Code generation for function 'kalman_dlqe3'
- *
- * C source code generated on: Tue Feb 19 15:26:30 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_TYPES_H__
-#define __KALMAN_DLQE3_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (kalman_dlqe3_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c deleted file mode 100755 index 5139848bc..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * positionKalmanFilter1D.c - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const - real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T - P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T - Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], - real32_T P_aposteriori[9]) -{ - int32_T i0; - real32_T f0; - int32_T k; - real32_T b_A[9]; - int32_T i1; - real32_T P_apriori[9]; - real32_T y; - real32_T K[3]; - real32_T S; - int8_T I[9]; - - /* prediction */ - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (k = 0; k < 3; k++) { - f0 += A[i0 + 3 * k] * x_aposteriori_k[k]; - } - - x_aposteriori[i0] = f0 + B[i0] * u; - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - b_A[i0 + 3 * k] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k]; - } - } - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1]; - } - - P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k]; - } - } - - if ((real32_T)fabs(u) < thresh) { - x_aposteriori[1] *= decay; - } - - /* update */ - if (gps_update == 1) { - y = 0.0F; - for (k = 0; k < 3; k++) { - y += C[k] * x_aposteriori[k]; - K[k] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - K[k] += C[i0] * P_apriori[i0 + 3 * k]; - } - } - - y = z - y; - S = 0.0F; - for (k = 0; k < 3; k++) { - S += K[k] * C[k]; - } - - S += R; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (k = 0; k < 3; k++) { - f0 += P_apriori[i0 + 3 * k] * C[k]; - } - - K[i0] = f0 / S; - } - - for (i0 = 0; i0 < 3; i0++) { - x_aposteriori[i0] += K[i0] * y; - } - - for (i0 = 0; i0 < 9; i0++) { - I[i0] = 0; - } - - for (k = 0; k < 3; k++) { - I[k + 3 * k] = 1; - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - P_aposteriori[i0 + 3 * k] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k]; - } - } - } - } else { - for (i0 = 0; i0 < 9; i0++) { - P_aposteriori[i0] = P_apriori[i0]; - } - } -} - -/* End of code generation (positionKalmanFilter1D.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h deleted file mode 100755 index 205c8eb4e..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D.h - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_H__ -#define __POSITIONKALMANFILTER1D_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); -#endif -/* End of code generation (positionKalmanFilter1D.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c deleted file mode 100755 index 4c535618a..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c +++ /dev/null @@ -1,157 +0,0 @@ -/* - * positionKalmanFilter1D_dT.c - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], - const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, - const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T - x_aposteriori[3], real32_T P_aposteriori[9]) -{ - real32_T A[9]; - int32_T i; - static const int8_T iv0[3] = { 0, 0, 1 }; - - real32_T K[3]; - real32_T f0; - int32_T i0; - real32_T b_A[9]; - int32_T i1; - real32_T P_apriori[9]; - static const int8_T iv1[3] = { 1, 0, 0 }; - - real32_T fv0[3]; - real32_T y; - static const int8_T iv2[3] = { 1, 0, 0 }; - - real32_T S; - int8_T I[9]; - - /* dynamics */ - A[0] = 1.0F; - A[3] = dT; - A[6] = -0.5F * dT * dT; - A[1] = 0.0F; - A[4] = 1.0F; - A[7] = -dT; - for (i = 0; i < 3; i++) { - A[2 + 3 * i] = (real32_T)iv0[i]; - } - - /* prediction */ - K[0] = 0.5F * dT * dT; - K[1] = dT; - K[2] = 0.0F; - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += A[i + 3 * i0] * x_aposteriori_k[i0]; - } - - x_aposteriori[i] = f0 + K[i] * u; - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1]; - } - - P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0]; - } - } - - if ((real32_T)fabs(u) < thresh) { - x_aposteriori[1] *= decay; - } - - /* update */ - if (gps_update == 1) { - f0 = 0.0F; - for (i = 0; i < 3; i++) { - f0 += (real32_T)iv1[i] * x_aposteriori[i]; - fv0[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i]; - } - } - - y = z - f0; - f0 = 0.0F; - for (i = 0; i < 3; i++) { - f0 += fv0[i] * (real32_T)iv2[i]; - } - - S = f0 + (real32_T)R; - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0]; - } - - K[i] = f0 / S; - } - - for (i = 0; i < 3; i++) { - x_aposteriori[i] += K[i] * y; - } - - for (i = 0; i < 9; i++) { - I[i] = 0; - } - - for (i = 0; i < 3; i++) { - I[i + 3 * i] = 1; - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - P_aposteriori[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0]; - } - } - } - } else { - for (i = 0; i < 9; i++) { - P_aposteriori[i] = P_apriori[i]; - } - } -} - -/* End of code generation (positionKalmanFilter1D_dT.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h deleted file mode 100755 index 94cbe2ce6..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT.h - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_H__ -#define __POSITIONKALMANFILTER1D_DT_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); -#endif -/* End of code generation (positionKalmanFilter1D_dT.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c deleted file mode 100755 index aa89f8a9d..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_initialize.c - * - * Code generation for function 'positionKalmanFilter1D_dT_initialize' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" -#include "positionKalmanFilter1D_dT_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h deleted file mode 100755 index 8d358a9a3..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_initialize.h - * - * Code generation for function 'positionKalmanFilter1D_dT_initialize' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ -#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT_initialize(void); -#endif -/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c deleted file mode 100755 index 20ed2edbb..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_terminate.c - * - * Code generation for function 'positionKalmanFilter1D_dT_terminate' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" -#include "positionKalmanFilter1D_dT_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h deleted file mode 100755 index 5eb5807a0..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_terminate.h - * - * Code generation for function 'positionKalmanFilter1D_dT_terminate' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ -#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT_terminate(void); -#endif -/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h deleted file mode 100755 index 43e5f016c..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * positionKalmanFilter1D_dT_types.h - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__ -#define __POSITIONKALMANFILTER1D_DT_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (positionKalmanFilter1D_dT_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c deleted file mode 100755 index 5bd87c390..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_initialize.c - * - * Code generation for function 'positionKalmanFilter1D_initialize' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" -#include "positionKalmanFilter1D_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (positionKalmanFilter1D_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h deleted file mode 100755 index 44bce472f..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_initialize.h - * - * Code generation for function 'positionKalmanFilter1D_initialize' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__ -#define __POSITIONKALMANFILTER1D_INITIALIZE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_initialize(void); -#endif -/* End of code generation (positionKalmanFilter1D_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c deleted file mode 100755 index 41e11936f..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_terminate.c - * - * Code generation for function 'positionKalmanFilter1D_terminate' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" -#include "positionKalmanFilter1D_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (positionKalmanFilter1D_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h deleted file mode 100755 index e84ea01bc..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_terminate.h - * - * Code generation for function 'positionKalmanFilter1D_terminate' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__ -#define __POSITIONKALMANFILTER1D_TERMINATE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_terminate(void); -#endif -/* End of code generation (positionKalmanFilter1D_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h deleted file mode 100755 index 4b473f56f..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * positionKalmanFilter1D_types.h - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_TYPES_H__ -#define __POSITIONKALMANFILTER1D_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (positionKalmanFilter1D_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/randn.c b/src/modules/position_estimator_mc/codegen/randn.c deleted file mode 100755 index 51aef7b76..000000000 --- a/src/modules/position_estimator_mc/codegen/randn.c +++ /dev/null @@ -1,524 +0,0 @@ -/*
- * randn.c
- *
- * Code generation for function 'randn'
- *
- * C source code generated on: Tue Feb 19 15:26:32 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "randn.h"
-#include "kalman_dlqe3_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-static uint32_T d_state[625];
-
-/* Function Declarations */
-static real_T b_genrandu(uint32_T mt[625]);
-static real_T eml_rand_mt19937ar(uint32_T e_state[625]);
-static real_T eml_rand_shr3cong(uint32_T e_state[2]);
-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);
-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);
-static void twister_state_vector(uint32_T mt[625], real_T seed);
-
-/* Function Definitions */
-static real_T b_genrandu(uint32_T mt[625])
-{
- real_T r;
- int32_T exitg1;
- uint32_T u[2];
- boolean_T isvalid;
- int32_T k;
- boolean_T exitg2;
-
- /* <LEGAL> This is a uniform (0,1) pseudorandom number generator based on: */
- /* <LEGAL> */
- /* <LEGAL> A C-program for MT19937, with initialization improved 2002/1/26. */
- /* <LEGAL> Coded by Takuji Nishimura and Makoto Matsumoto. */
- /* <LEGAL> */
- /* <LEGAL> Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */
- /* <LEGAL> All rights reserved. */
- /* <LEGAL> */
- /* <LEGAL> Redistribution and use in source and binary forms, with or without */
- /* <LEGAL> modification, are permitted provided that the following conditions */
- /* <LEGAL> are met: */
- /* <LEGAL> */
- /* <LEGAL> 1. Redistributions of source code must retain the above copyright */
- /* <LEGAL> notice, this list of conditions and the following disclaimer. */
- /* <LEGAL> */
- /* <LEGAL> 2. Redistributions in binary form must reproduce the above copyright */
- /* <LEGAL> notice, this list of conditions and the following disclaimer in the */
- /* <LEGAL> documentation and/or other materials provided with the distribution. */
- /* <LEGAL> */
- /* <LEGAL> 3. The names of its contributors may not be used to endorse or promote */
- /* <LEGAL> products derived from this software without specific prior written */
- /* <LEGAL> permission. */
- /* <LEGAL> */
- /* <LEGAL> THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
- /* <LEGAL> "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
- /* <LEGAL> LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */
- /* <LEGAL> A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */
- /* <LEGAL> CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */
- /* <LEGAL> EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */
- /* <LEGAL> PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */
- /* <LEGAL> PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */
- /* <LEGAL> LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */
- /* <LEGAL> NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
- /* <LEGAL> SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
- do {
- exitg1 = 0;
- genrand_uint32_vector(mt, u);
- r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)
- (u[1] >> 6U));
- if (r == 0.0) {
- if ((mt[624] >= 1U) && (mt[624] < 625U)) {
- isvalid = TRUE;
- } else {
- isvalid = FALSE;
- }
-
- if (isvalid) {
- isvalid = FALSE;
- k = 1;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (k < 625)) {
- if (mt[k - 1] == 0U) {
- k++;
- } else {
- isvalid = TRUE;
- exitg2 = TRUE;
- }
- }
- }
-
- if (!isvalid) {
- twister_state_vector(mt, 5489.0);
- }
- } else {
- exitg1 = 1;
- }
- } while (exitg1 == 0);
-
- return r;
-}
-
-static real_T eml_rand_mt19937ar(uint32_T e_state[625])
-{
- real_T r;
- int32_T exitg1;
- uint32_T u32[2];
- int32_T i;
- static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,
- 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
- 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
- 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
- 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
- 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
- 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
- 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
- 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
- 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
- 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
- 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
- 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
- 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
- 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
- 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
- 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
- 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
- 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
- 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
- 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
- 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
- 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
- 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
- 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
- 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
- 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
- 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
- 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
- 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
- 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
- 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
- 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
- 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
- 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
- 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
- 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
- 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
- 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
- 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
- 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
- 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
- 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
- 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
- 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
- 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
- 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
- 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
- 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
- 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
- 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
- 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
- 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
- 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
- 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
- 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
- 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
- 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
- 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
- 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
- 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
- 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
- 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
- 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
- 3.65415288536101, 3.91075795952492 };
-
- real_T u;
- static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,
- 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
- 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
- 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
- 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
- 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
- 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
- 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
- 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
- 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
- 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
- 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
- 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
- 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
- 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
- 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
- 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
- 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
- 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
- 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
- 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
- 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
- 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
- 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
- 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
- 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
- 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
- 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
- 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
- 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
- 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
- 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
- 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
- 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
- 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
- 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
- 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
- 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
- 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
- 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
- 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
- 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
- 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
- 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
- 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
- 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
- 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
- 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
- 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
- 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
- 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
- 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
- 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
- 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
- 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
- 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
- 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
- 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
- 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
- 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
- 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
- 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
- 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
- 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
- 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
- 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
- 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
- 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
- 0.000477467764609386 };
-
- real_T x;
- do {
- exitg1 = 0;
- genrand_uint32_vector(e_state, u32);
- i = (int32_T)((u32[1] >> 24U) + 1U);
- r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
- 16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];
- if (fabs(r) <= dv1[i - 1]) {
- exitg1 = 1;
- } else if (i < 256) {
- u = b_genrandu(e_state);
- if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {
- exitg1 = 1;
- }
- } else {
- do {
- u = b_genrandu(e_state);
- x = log(u) * 0.273661237329758;
- u = b_genrandu(e_state);
- } while (!(-2.0 * log(u) > x * x));
-
- if (r < 0.0) {
- r = x - 3.65415288536101;
- } else {
- r = 3.65415288536101 - x;
- }
-
- exitg1 = 1;
- }
- } while (exitg1 == 0);
-
- return r;
-}
-
-static real_T eml_rand_shr3cong(uint32_T e_state[2])
-{
- real_T r;
- uint32_T icng;
- uint32_T jsr;
- uint32_T ui;
- int32_T j;
- static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,
- 0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,
- 0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,
- 1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,
- 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,
- 1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,
- 1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,
- 1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,
- 2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
- 2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };
-
- real_T x;
- real_T y;
- real_T s;
- icng = 69069U * e_state[0] + 1234567U;
- jsr = e_state[1] ^ e_state[1] << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- ui = icng + jsr;
- j = (int32_T)(ui & 63U);
- r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];
- if (fabs(r) <= dv0[j]) {
- } else {
- x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);
- icng = 69069U * icng + 1234567U;
- jsr ^= jsr << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;
- s = x + (0.5 + y);
- if (s > 1.301198) {
- if (r < 0.0) {
- r = 0.4878992 * x - 0.4878992;
- } else {
- r = 0.4878992 - 0.4878992 * x;
- }
- } else if (s <= 0.9689279) {
- } else {
- s = 0.4878992 * x;
- x = 0.4878992 - 0.4878992 * x;
- if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {
- if (r < 0.0) {
- r = -(0.4878992 - s);
- } else {
- r = 0.4878992 - s;
- }
- } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /
- dv0[j + 1] <= exp(-0.5 * r * r)) {
- } else {
- do {
- icng = 69069U * icng + 1234567U;
- jsr ^= jsr << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /
- 2.776994;
- icng = 69069U * icng + 1234567U;
- jsr ^= jsr << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *
- 2.328306436538696E-10) > x * x));
-
- if (r < 0.0) {
- r = x - 2.776994;
- } else {
- r = 2.776994 - x;
- }
- }
- }
- }
-
- e_state[0] = icng;
- e_state[1] = jsr;
- return r;
-}
-
-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])
-{
- int32_T i;
- uint32_T mti;
- int32_T kk;
- uint32_T y;
- uint32_T b_y;
- uint32_T c_y;
- uint32_T d_y;
- for (i = 0; i < 2; i++) {
- u[i] = 0U;
- }
-
- for (i = 0; i < 2; i++) {
- mti = mt[624] + 1U;
- if (mti >= 625U) {
- for (kk = 0; kk < 227; kk++) {
- y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- b_y = y >> 1U;
- } else {
- b_y = y >> 1U ^ 2567483615U;
- }
-
- mt[kk] = mt[397 + kk] ^ b_y;
- }
-
- for (kk = 0; kk < 396; kk++) {
- y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- c_y = y >> 1U;
- } else {
- c_y = y >> 1U ^ 2567483615U;
- }
-
- mt[227 + kk] = mt[kk] ^ c_y;
- }
-
- y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- d_y = y >> 1U;
- } else {
- d_y = y >> 1U ^ 2567483615U;
- }
-
- mt[623] = mt[396] ^ d_y;
- mti = 1U;
- }
-
- y = mt[(int32_T)mti - 1];
- mt[624] = mti;
- y ^= y >> 11U;
- y ^= y << 7U & 2636928640U;
- y ^= y << 15U & 4022730752U;
- y ^= y >> 18U;
- u[i] = y;
- }
-}
-
-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)
-{
- int32_T hi;
- uint32_T test1;
- uint32_T test2;
- hi = (int32_T)(s / 127773U);
- test1 = 16807U * (s - (uint32_T)hi * 127773U);
- test2 = 2836U * (uint32_T)hi;
- if (test1 < test2) {
- *e_state = (test1 - test2) + 2147483647U;
- } else {
- *e_state = test1 - test2;
- }
-
- *r = (real_T)*e_state * 4.6566128752457969E-10;
-}
-
-static void twister_state_vector(uint32_T mt[625], real_T seed)
-{
- uint32_T r;
- int32_T mti;
- if (seed < 4.294967296E+9) {
- if (seed >= 0.0) {
- r = (uint32_T)seed;
- } else {
- r = 0U;
- }
- } else if (seed >= 4.294967296E+9) {
- r = MAX_uint32_T;
- } else {
- r = 0U;
- }
-
- mt[0] = r;
- for (mti = 0; mti < 623; mti++) {
- r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
- mt[1 + mti] = r;
- }
-
- mt[624] = 624U;
-}
-
-real_T randn(void)
-{
- real_T r;
- uint32_T e_state;
- real_T t;
- real_T b_r;
- uint32_T f_state;
- if (method == 0U) {
- if (b_method == 4U) {
- do {
- genrandu(b_state, &e_state, &r);
- genrandu(e_state, &b_state, &t);
- b_r = 2.0 * r - 1.0;
- t = 2.0 * t - 1.0;
- t = t * t + b_r * b_r;
- } while (!(t <= 1.0));
-
- r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
- } else if (b_method == 5U) {
- r = eml_rand_shr3cong(c_state);
- } else {
- if (!state_not_empty) {
- memset(&d_state[0], 0, 625U * sizeof(uint32_T));
- twister_state_vector(d_state, 5489.0);
- state_not_empty = TRUE;
- }
-
- r = eml_rand_mt19937ar(d_state);
- }
- } else if (method == 4U) {
- e_state = state[0];
- do {
- genrandu(e_state, &f_state, &r);
- genrandu(f_state, &e_state, &t);
- b_r = 2.0 * r - 1.0;
- t = 2.0 * t - 1.0;
- t = t * t + b_r * b_r;
- } while (!(t <= 1.0));
-
- state[0] = e_state;
- r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
- } else {
- r = eml_rand_shr3cong(state);
- }
-
- return r;
-}
-
-/* End of code generation (randn.c) */
diff --git a/src/modules/position_estimator_mc/codegen/randn.h b/src/modules/position_estimator_mc/codegen/randn.h deleted file mode 100755 index 8a2aa9277..000000000 --- a/src/modules/position_estimator_mc/codegen/randn.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * randn.h
- *
- * Code generation for function 'randn'
- *
- * C source code generated on: Tue Feb 19 15:26:32 2013
- *
- */
-
-#ifndef __RANDN_H__
-#define __RANDN_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real_T randn(void);
-#endif
-/* End of code generation (randn.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.c b/src/modules/position_estimator_mc/codegen/rtGetInf.c deleted file mode 100755 index c6fa7884e..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.h b/src/modules/position_estimator_mc/codegen/rtGetInf.h deleted file mode 100755 index e7b2a2d1c..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include <stddef.h> -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.c b/src/modules/position_estimator_mc/codegen/rtGetNaN.c deleted file mode 100755 index 552770149..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.h b/src/modules/position_estimator_mc/codegen/rtGetNaN.h deleted file mode 100755 index 5acdd9790..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include <stddef.h> -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c deleted file mode 100755 index de121c4a0..000000000 --- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h deleted file mode 100755 index 3bbcfd087..000000000 --- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include <float.h> -#endif -#include <stddef.h> -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtwtypes.h b/src/modules/position_estimator_mc/codegen/rtwtypes.h deleted file mode 100755 index 8916e8572..000000000 --- a/src/modules/position_estimator_mc/codegen/rtwtypes.h +++ /dev/null @@ -1,159 +0,0 @@ -/* - * rtwtypes.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include <limits.h> - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Undefined - * Shift right on a signed integer as arithmetic shift: off - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/position_estimator_mc/kalman_dlqe1.m b/src/modules/position_estimator_mc/kalman_dlqe1.m deleted file mode 100755 index ff939d029..000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe1.m +++ /dev/null @@ -1,3 +0,0 @@ -function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z) - x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); -end
\ No newline at end of file diff --git a/src/modules/position_estimator_mc/kalman_dlqe2.m b/src/modules/position_estimator_mc/kalman_dlqe2.m deleted file mode 100755 index 2a50164ef..000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe2.m +++ /dev/null @@ -1,9 +0,0 @@ -function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z) - st = 1/2*dt^2; - A = [1,dt,st; - 0,1,dt; - 0,0,1]; - C=[1,0,0]; - K = [k1;k2;k3]; - x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); -end
\ No newline at end of file diff --git a/src/modules/position_estimator_mc/kalman_dlqe3.m b/src/modules/position_estimator_mc/kalman_dlqe3.m deleted file mode 100755 index 4c6421b7f..000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe3.m +++ /dev/null @@ -1,17 +0,0 @@ -function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)
- st = 1/2*dt^2;
- A = [1,dt,st;
- 0,1,dt;
- 0,0,1];
- C=[1,0,0];
- K = [k1;k2;k3];
- if addNoise==1
- noise = sigma*randn(1,1);
- z = z + noise;
- end
- if(posUpdate)
- x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
- else
- x_aposteriori=A*x_aposteriori_k;
- end
-end
\ No newline at end of file diff --git a/src/modules/position_estimator_mc/module.mk b/src/modules/position_estimator_mc/module.mk deleted file mode 100644 index 40b135ea4..000000000 --- a/src/modules/position_estimator_mc/module.mk +++ /dev/null @@ -1,60 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the position estimator -# - -MODULE_COMMAND = position_estimator_mc - -SRCS = position_estimator_mc_main.c \ - position_estimator_mc_params.c \ - codegen/positionKalmanFilter1D_initialize.c \ - codegen/positionKalmanFilter1D_terminate.c \ - codegen/positionKalmanFilter1D.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/positionKalmanFilter1D_dT_initialize.c \ - codegen/positionKalmanFilter1D_dT_terminate.c \ - codegen/kalman_dlqe1.c \ - codegen/kalman_dlqe1_initialize.c \ - codegen/kalman_dlqe1_terminate.c \ - codegen/kalman_dlqe2.c \ - codegen/kalman_dlqe2_initialize.c \ - codegen/kalman_dlqe2_terminate.c \ - codegen/kalman_dlqe3.c \ - codegen/kalman_dlqe3_initialize.c \ - codegen/kalman_dlqe3_terminate.c \ - codegen/kalman_dlqe3_data.c \ - codegen/randn.c diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D.m b/src/modules/position_estimator_mc/positionKalmanFilter1D.m deleted file mode 100755 index 144ff8c7c..000000000 --- a/src/modules/position_estimator_mc/positionKalmanFilter1D.m +++ /dev/null @@ -1,19 +0,0 @@ -function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) -%prediction - x_apriori=A*x_aposteriori_k+B*u; - P_apriori=A*P_aposteriori_k*A'+Q; - if abs(u)<thresh - x_apriori(2)=decay*x_apriori(2); - end - %update - if gps_update==1 - y=z-C*x_apriori; - S=C*P_apriori*C'+R; - K=(P_apriori*C')/S; - x_aposteriori=x_apriori+K*y; - P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori; - else - x_aposteriori=x_apriori; - P_aposteriori=P_apriori; - end -end diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m deleted file mode 100755 index f94cce1fb..000000000 --- a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m +++ /dev/null @@ -1,26 +0,0 @@ -function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) - %dynamics - A = [1 dT -0.5*dT*dT; - 0 1 -dT; - 0 0 1]; - B = [0.5*dT*dT; dT; 0]; - C = [1 0 0]; - %prediction - x_apriori=A*x_aposteriori_k+B*u; - P_apriori=A*P_aposteriori_k*A'+Q; - if abs(u)<thresh - x_apriori(2)=decay*x_apriori(2); - end - %update - if gps_update==1 - y=z-C*x_apriori; - S=C*P_apriori*C'+R; - K=(P_apriori*C')/S; - x_aposteriori=x_apriori+K*y; - P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori; - else - x_aposteriori=x_apriori; - P_aposteriori=P_apriori; - end -end - diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c deleted file mode 100755 index 363961819..000000000 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ /dev/null @@ -1,515 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger <daregger@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> -* Lorenz Meier <lm@inf.ethz.ch> - * - * 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 position_estimator_main.c - * Model-identification based position estimator for multirotors - */ - -#include <unistd.h> -#include <stdlib.h> -#include <stdio.h> -#include <stdbool.h> -#include <fcntl.h> -#include <float.h> -#include <string.h> -#include <nuttx/config.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <limits.h> -#include <math.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls_effective.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <mavlink/mavlink_log.h> -#include <poll.h> -#include <systemlib/geo/geo.h> -#include <systemlib/err.h> -#include <systemlib/systemlib.h> - -#include <drivers/drv_hrt.h> - -#include "position_estimator_mc_params.h" -//#include <uORB/topics/debug_key_value.h> -#include "codegen/kalman_dlqe2.h" -#include "codegen/kalman_dlqe2_initialize.h" -#include "codegen/kalman_dlqe3.h" -#include "codegen/kalman_dlqe3_initialize.h" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int position_estimator_mc_task; /**< Handle of deamon task / thread */ - -__EXPORT int position_estimator_mc_main(int argc, char *argv[]); - -int position_estimator_mc_thread_main(int argc, char *argv[]); -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - warnx("%s\n", reason); - warnx("usage: position_estimator_mc {start|stop|status}"); - exit(1); -} - -/** - * The position_estimator_mc_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int position_estimator_mc_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("position_estimator_mc already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - position_estimator_mc_task = task_spawn_cmd("position_estimator_mc", - SCHED_RR, - SCHED_PRIORITY_MAX - 5, - 4096, - position_estimator_mc_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("position_estimator_mc is running"); - } else { - warnx("position_estimator_mc not started"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/**************************************************************************** - * main - ****************************************************************************/ -int position_estimator_mc_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("[position_estimator_mc] started"); - int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[position_estimator_mc] started"); - - /* initialize values */ - float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */ - // float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f}; - float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f}; - float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f}; - float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f}; - - // XXX this is terribly wrong and should actual dT instead - const float dT_const_50 = 1.0f/50.0f; - - float addNoise = 0.0f; - float sigma = 0.0f; - //computed from dlqe in matlab - const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f}; - // XXX implement baro filter - const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f}; - float K[3] = {0.0f, 0.0f, 0.0f}; - int baro_loop_cnt = 0; - int baro_loop_end = 70; /* measurement for 1 second */ - float p0_Pa = 0.0f; /* to determin while start up */ - float rho0 = 1.293f; /* standard pressure */ - const float const_earth_gravity = 9.81f; - - float posX = 0.0f; - float posY = 0.0f; - float posZ = 0.0f; - - double lat_current; - double lon_current; - float alt_current; - - float gps_origin_altitude = 0.0f; - - /* Initialize filter */ - kalman_dlqe2_initialize(); - kalman_dlqe3_initialize(); - - /* declare and safely initialize all structs */ - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ - struct vehicle_vicon_position_s vicon_pos; - memset(&vicon_pos, 0, sizeof(vicon_pos)); - struct actuator_controls_effective_s act_eff; - memset(&act_eff, 0, sizeof(act_eff)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); - struct vehicle_local_position_s local_pos_est; - memset(&local_pos_est, 0, sizeof(local_pos_est)); - struct vehicle_global_position_s global_pos_est; - memset(&global_pos_est, 0, sizeof(global_pos_est)); - - /* subscribe */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0)); - int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* advertise */ - orb_advert_t local_pos_est_pub = 0; - orb_advert_t global_pos_est_pub = 0; - - struct position_estimator_mc_params pos1D_params; - struct position_estimator_mc_param_handles pos1D_param_handles; - /* initialize parameter handles */ - parameters_init(&pos1D_param_handles); - - bool flag_use_gps = false; - bool flag_use_baro = false; - bool flag_baro_initialized = false; /* in any case disable baroINITdone */ - /* FIRST PARAMETER READ at START UP*/ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */ - /* FIRST PARAMETER UPDATE */ - parameters_update(&pos1D_param_handles, &pos1D_params); - flag_use_baro = pos1D_params.baro; - sigma = pos1D_params.sigma; - addNoise = pos1D_params.addNoise; - /* END FIRST PARAMETER UPDATE */ - - /* try to grab a vicon message - if it fails, go for GPS. */ - - /* make sure the next orb_check() can't return true unless we get a timely update */ - orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); - /* allow 200 ms for vicon to come in */ - usleep(200000); - /* check if we got vicon */ - bool update_check; - orb_check(vicon_pos_sub, &update_check); - /* if no update was available, use GPS */ - flag_use_gps = !update_check; - - if (flag_use_gps) { - mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked"); - /* wait until gps signal turns valid, only then can we initialize the projection */ - - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - while (!(gps.fix_type == 3 - && (gps.eph_m < hdop_threshold_m) - && (gps.epv_m < vdop_threshold_m) - && (hrt_absolute_time() - gps.timestamp_position < 2000000))) { - - struct pollfd fds1[2] = { - { .fd = vehicle_gps_sub, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN }, - }; - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds1, 2, 5000)) { - if (fds1[0].revents & POLLIN){ - /* Read gps position */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - } - if (fds1[1].revents & POLLIN){ - /* Read out parameters to check for an update there, e.g. useGPS variable */ - /* read from param to clear updated flag */ - struct parameter_update_s updated; - orb_copy(ORB_ID(parameter_update), sub_params, &updated); - /* update parameters */ - parameters_update(&pos1D_param_handles, &pos1D_params); - } - } - static int printcounter = 0; - if (printcounter == 100) { - printcounter = 0; - warnx("[pos_est_mc] wait for GPS fix"); - } - printcounter++; - } - - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7d; - lon_current = ((double)(gps.lon)) * 1e-7d; - alt_current = gps.alt * 1e-3f; - gps_origin_altitude = alt_current; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ - printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - - } else { - mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); - /* onboard calculated position estimations */ - } - thread_running = true; - - struct pollfd fds2[3] = { - { .fd = vehicle_gps_sub, .events = POLLIN }, - { .fd = vicon_pos_sub, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN }, - }; - - bool vicon_updated = false; - bool gps_updated = false; - - /**< main_loop */ - while (!thread_should_exit) { - int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate - if (ret < 0) { - /* poll error */ - } else { - if (fds2[2].revents & POLLIN){ - /* new parameter */ - /* read from param to clear updated flag */ - struct parameter_update_s updated; - orb_copy(ORB_ID(parameter_update), sub_params, &updated); - /* update parameters */ - parameters_update(&pos1D_param_handles, &pos1D_params); - flag_use_baro = pos1D_params.baro; - sigma = pos1D_params.sigma; - addNoise = pos1D_params.addNoise; - } - vicon_updated = false; /* default is no vicon_updated */ - if (fds2[1].revents & POLLIN) { - /* new vicon position */ - orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); - posX = vicon_pos.x; - posY = vicon_pos.y; - posZ = vicon_pos.z; - vicon_updated = true; /* set flag for vicon update */ - } /* end of poll call for vicon updates */ - gps_updated = false; - if (fds2[0].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - /* Project gps lat lon (Geographic coordinate system) to plane*/ - map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1])); - posX = z[0]; - posY = z[1]; - posZ = (float)(gps.alt * 1e-3f); - gps_updated = true; - } - - /* Main estimator loop */ - orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff); - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor); - // barometric pressure estimation at start up - if (!flag_baro_initialized){ - // mean calculation over several measurements - if (baro_loop_cnt<baro_loop_end) { - p0_Pa += (sensor.baro_pres_mbar*100); - baro_loop_cnt++; - } else { - p0_Pa /= (float)(baro_loop_cnt); - flag_baro_initialized = true; - char *baro_m_start = "barometer initialized with p0 = "; - char p0_char[15]; - sprintf(p0_char, "%8.2f", (double)(p0_Pa/100)); - char *baro_m_end = " mbar"; - char str[80]; - strcpy(str,baro_m_start); - strcat(str,p0_char); - strcat(str,baro_m_end); - mavlink_log_info(mavlink_fd, str); - } - } - if (flag_use_gps) { - /* initialize map projection with the last estimate (not at full rate) */ - if (gps.fix_type > 2) { - /* x-y-position/velocity estimation in earth frame = gps frame */ - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori); - memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori); - memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); - /* z-position/velocity estimation in earth frame = vicon frame */ - float z_est = 0.0f; - if (flag_baro_initialized && flag_use_baro) { - z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity); - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */ - } else { - z_est = posZ; - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - } - - kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori); - memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); - local_pos_est.x = x_x_aposteriori_k[0]; - local_pos_est.vx = x_x_aposteriori_k[1]; - local_pos_est.y = x_y_aposteriori_k[0]; - local_pos_est.vy = x_y_aposteriori_k[1]; - local_pos_est.z = x_z_aposteriori_k[0]; - local_pos_est.vz = x_z_aposteriori_k[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) { - /* publish local position estimate */ - if (local_pos_est_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); - } else { - local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); - } - /* publish on GPS updates */ - if (gps_updated) { - - double lat, lon; - float alt = z_est + gps_origin_altitude; - - map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon); - - global_pos_est.lat = lat; - global_pos_est.lon = lon; - global_pos_est.alt = alt; - - if (global_pos_est_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est); - } else { - global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est); - } - } - } - } - } else { - /* x-y-position/velocity estimation in earth frame = vicon frame */ - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori); - memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori); - memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); - /* z-position/velocity estimation in earth frame = vicon frame */ - float z_est = 0.0f; - float local_sigma = 0.0f; - if (flag_baro_initialized && flag_use_baro) { - z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity); - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */ - local_sigma = 0.0f; /* don't add noise on barometer in any case */ - } else { - z_est = posZ; - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - local_sigma = sigma; - } - kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori); - memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); - local_pos_est.x = x_x_aposteriori_k[0]; - local_pos_est.vx = x_x_aposteriori_k[1]; - local_pos_est.y = x_y_aposteriori_k[0]; - local_pos_est.vy = x_y_aposteriori_k[1]; - local_pos_est.z = x_z_aposteriori_k[0]; - local_pos_est.vz = x_z_aposteriori_k[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - if(local_pos_est_pub > 0) - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); - else - local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); - //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); - //mavlink_log_info(mavlink_fd, buf); - } - } - } /* end of poll return value check */ - } - - printf("[pos_est_mc] exiting.\n"); - mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting"); - thread_running = false; - return 0; -} diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.c b/src/modules/position_estimator_mc/position_estimator_mc_params.c deleted file mode 100755 index 72e5bc73b..000000000 --- a/src/modules/position_estimator_mc/position_estimator_mc_params.c +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger <daregger@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> -* Lorenz Meier <lm@inf.ethz.ch> - * - * 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 position_estimator_mc_params.c - * - * Parameters for position_estimator_mc - */ - -#include "position_estimator_mc_params.h" - -/* Kalman Filter covariances */ -/* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f); -PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f); -PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); -PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f); - -int parameters_init(struct position_estimator_mc_param_handles *h) -{ - h->addNoise = param_find("POS_EST_ADDN"); - h->sigma = param_find("POS_EST_SIGMA"); - h->r = param_find("POS_EST_R"); - h->baro_param_handle = param_find("POS_EST_BARO"); - return OK; -} - -int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p) -{ - param_get(h->addNoise, &(p->addNoise)); - param_get(h->sigma, &(p->sigma)); - param_get(h->r, &(p->R)); - param_get(h->baro_param_handle, &(p->baro)); - return OK; -} diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.h b/src/modules/position_estimator_mc/position_estimator_mc_params.h deleted file mode 100755 index c4c95f7b4..000000000 --- a/src/modules/position_estimator_mc/position_estimator_mc_params.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger <daregger@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * 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 position_estimator_mc_params.h - * - * Parameters for Position Estimator - */ - -#include <systemlib/param/param.h> - -struct position_estimator_mc_params { - float addNoise; - float sigma; - float R; - int baro; /* consider barometer */ -}; - -struct position_estimator_mc_param_handles { - param_t addNoise; - param_t sigma; - param_t r; - param_t baro_param_handle; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct position_estimator_mc_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9558198f3..ebf4f3e8e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -254,10 +254,25 @@ mixer_tick(void) for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + /* set S.BUS1 or S.BUS2 outputs */ + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + } + } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); + + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4db948484..ca175bfbc 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -219,6 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +extern bool sbus1_output(uint16_t *values, uint16_t num_values); +extern bool sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9e5d7e7e2..fd7c6081f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -463,9 +463,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); - /* disable the conflicting options */ - if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + /* disable the conflicting options with SBUS 1 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with SBUS 2 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } #endif diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index f6ec542eb..0e7dc621c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -93,7 +93,7 @@ int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY | O_NONBLOCK); + sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -113,11 +113,22 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } - return sbus_fd; } bool +sbus1_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'A', 1); +} + +bool +sbus2_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'B', 1); +} + +bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; diff --git a/src/modules/sdlog/module.mk b/src/modules/sdlog/module.mk deleted file mode 100644 index 89da2d827..000000000 --- a/src/modules/sdlog/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# sdlog Application -# - -MODULE_COMMAND = sdlog -# The main thread only buffers to RAM, needs a high priority -MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" - -SRCS = sdlog.c \ - sdlog_ringbuffer.c diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c deleted file mode 100644 index c22523bf2..000000000 --- a/src/modules/sdlog/sdlog.c +++ /dev/null @@ -1,840 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 sdlog.c - * @author Lorenz Meier <lm@inf.ethz.ch> - * - * Simple SD logger for flight data. Buffers new sensor values and - * does the heavy SD I/O in a low-priority worker thread. - */ - -#include <nuttx/config.h> -#include <sys/types.h> -#include <sys/stat.h> -#include <sys/prctl.h> -#include <fcntl.h> -#include <errno.h> -#include <unistd.h> -#include <stdio.h> -#include <poll.h> -#include <stdlib.h> -#include <string.h> -#include <ctype.h> -#include <systemlib/err.h> -#include <unistd.h> -#include <drivers/drv_hrt.h> - -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_controls_effective.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/optical_flow.h> -#include <uORB/topics/battery_status.h> -#include <uORB/topics/differential_pressure.h> -#include <uORB/topics/airspeed.h> - -#include <systemlib/systemlib.h> - -#include <mavlink/mavlink_log.h> - -#include "sdlog_ringbuffer.h" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ - -static const char *mountpoint = "/fs/microsd"; -static const char *mfile_in = "/etc/logging/logconv.m"; -int sysvector_file = -1; -int mavlink_fd = -1; -struct sdlog_logbuffer lb; - -/* mutex / condition to synchronize threads */ -pthread_mutex_t sysvector_mutex; -pthread_cond_t sysvector_cond; - -/** - * System state vector log buffer writing - */ -static void *sdlog_sysvector_write_thread(void *arg); - -/** - * Create the thread to write the system vector - */ -pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf); - -/** - * SD log management function. - */ -__EXPORT int sdlog_main(int argc, char *argv[]); - -/** - * Mainloop of sd log deamon. - */ -int sdlog_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static int file_exist(const char *filename); - -static int file_copy(const char *file_old, const char *file_new); - -static void handle_command(struct vehicle_command_s *cmd); - -/** - * Print the current status. - */ -static void print_sdlog_status(void); - -/** - * Create folder for current logging session. - */ -static int create_logfolder(char *folder_path); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n"); -} - -// XXX turn this into a C++ class -unsigned sensor_combined_bytes = 0; -unsigned actuator_outputs_bytes = 0; -unsigned actuator_controls_bytes = 0; -unsigned sysvector_bytes = 0; -unsigned blackbox_file_bytes = 0; -uint64_t starttime = 0; - -/* logging on or off, default to true */ -bool logging_enabled = true; - -/** - * The sd log deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn_cmd(). - */ -int sdlog_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("sdlog already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("sdlog", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 4096, - sdlog_thread_main, - (const char **)argv); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (!thread_running) { - printf("\tsdlog is not started\n"); - } - - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - print_sdlog_status(); - - } else { - printf("\tsdlog not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int create_logfolder(char *folder_path) -{ - /* make folder on sdcard */ - uint16_t foldernumber = 1; // start with folder 0001 - int mkdir_ret; - - /* look for the next folder that does not exist */ - while (foldernumber < MAX_NO_LOGFOLDER) { - /* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */ - sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ - - if (mkdir_ret == 0) { - /* folder does not exist, success */ - - /* now copy the Matlab/Octave file */ - char mfile_out[100]; - sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber); - int ret = file_copy(mfile_in, mfile_out); - - if (!ret) { - warnx("copied m file to %s", mfile_out); - - } else { - warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out); - } - - break; - - } else if (mkdir_ret == -1) { - /* folder exists already */ - foldernumber++; - continue; - - } else { - warn("failed creating new folder"); - return -1; - } - } - - if (foldernumber >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); - return -1; - } - - return 0; -} - - -static void * -sdlog_sysvector_write_thread(void *arg) -{ - /* set name */ - prctl(PR_SET_NAME, "sdlog microSD I/O", 0); - - struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg; - - int poll_count = 0; - struct sdlog_sysvector sysvect; - memset(&sysvect, 0, sizeof(sysvect)); - - while (!thread_should_exit) { - - /* make sure threads are synchronized */ - pthread_mutex_lock(&sysvector_mutex); - - /* only wait if no data is available to process */ - if (sdlog_logbuffer_is_empty(logbuf)) { - /* blocking wait for new data at this line */ - pthread_cond_wait(&sysvector_cond, &sysvector_mutex); - } - - /* only quickly load data, do heavy I/O a few lines down */ - int ret = sdlog_logbuffer_read(logbuf, &sysvect); - /* continue */ - pthread_mutex_unlock(&sysvector_mutex); - - if (ret == OK) { - sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect)); - } - - if (poll_count % 100 == 0) { - fsync(sysvector_file); - } - - poll_count++; - } - - fsync(sysvector_file); - - return OK; -} - -pthread_t -sysvector_write_start(struct sdlog_logbuffer *logbuf) -{ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - - struct sched_param param; - /* low priority, as this is expensive disk I/O */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - - pthread_attr_setstacksize(&receiveloop_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf); - return thread; - - // XXX we have to destroy the attr at some point -} - - -int sdlog_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.\n"); - } - - /* log every n'th value (skip three per default) */ - int skip_value = 3; - - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - int ch; - - while ((ch = getopt(argc, argv, "s:r")) != EOF) { - switch (ch) { - case 's': - { - /* log only every n'th (gyro clocked) value */ - unsigned s = strtoul(optarg, NULL, 10); - - if (s < 1 || s > 250) { - errx(1, "Wrong skip value of %d, out of range (1..250)\n", s); - } else { - skip_value = s; - } - } - break; - - case 'r': - /* log only on request, disable logging per default */ - logging_enabled = false; - break; - - case '?': - if (optopt == 'c') { - warnx("Option -%c requires an argument.\n", optopt); - } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.\n", optopt); - } else { - warnx("Unknown option character `\\x%x'.\n", optopt); - } - - default: - usage("unrecognized flag"); - errx(1, "exiting."); - } - } - - if (file_exist(mountpoint) != OK) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); - } - - char folder_path[64]; - - if (create_logfolder(folder_path)) - errx(1, "unable to create logging folder, exiting."); - - FILE *gpsfile; - FILE *blackbox_file; - - /* string to hold the path to the sensorfile */ - char path_buf[64] = ""; - - /* only print logging path, important to find log file later */ - warnx("logging to directory %s\n", folder_path); - - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); - - if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } - - /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ - sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); - - if (NULL == (gpsfile = fopen(path_buf, "w"))) { - errx(1, "opening %s failed.\n", path_buf); - } - - int gpsfile_no = fileno(gpsfile); - - /* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */ - sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox"); - - if (NULL == (blackbox_file = fopen(path_buf, "w"))) { - errx(1, "opening %s failed.\n", path_buf); - } - - // XXX for fsync() calls - int blackbox_file_no = fileno(blackbox_file); - - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 25; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - - - struct { - struct sensor_combined_s raw; - struct vehicle_attitude_s att; - struct vehicle_attitude_setpoint_s att_sp; - struct actuator_outputs_s act_outputs; - struct actuator_controls_s act_controls; - struct actuator_controls_effective_s act_controls_effective; - struct vehicle_command_s cmd; - struct vehicle_local_position_s local_pos; - struct vehicle_global_position_s global_pos; - struct vehicle_gps_position_s gps_pos; - struct vehicle_vicon_position_s vicon_pos; - struct optical_flow_s flow; - struct battery_status_s batt; - struct differential_pressure_s diff_pres; - struct airspeed_s airspeed; - } buf; - memset(&buf, 0, sizeof(buf)); - - struct { - int cmd_sub; - int sensor_sub; - int att_sub; - int spa_sub; - int act_0_sub; - int controls_0_sub; - int controls_effective_0_sub; - int local_pos_sub; - int global_pos_sub; - int gps_pos_sub; - int vicon_pos_sub; - int flow_sub; - int batt_sub; - int diff_pres_sub; - int airspeed_sub; - } subs; - - /* --- MANAGEMENT - LOGGING COMMAND --- */ - /* subscribe to ORB for vehicle command */ - subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - fds[fdsc_count].fd = subs.cmd_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS POSITION --- */ - /* subscribe to ORB for global position */ - subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - fds[fdsc_count].fd = subs.gps_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = subs.sensor_sub; - /* do not rate limit, instead use skip counter (aliasing on rate limit) */ - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ - subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - fds[fdsc_count].fd = subs.att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ - subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - fds[fdsc_count].fd = subs.spa_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- ACTUATOR OUTPUTS --- */ - subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs.act_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.controls_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.controls_effective_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POSITION --- */ - /* subscribe to ORB for local position */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - fds[fdsc_count].fd = subs.local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POSITION --- */ - /* subscribe to ORB for global position */ - subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - fds[fdsc_count].fd = subs.global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- VICON POSITION --- */ - /* subscribe to ORB for vicon position */ - subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - fds[fdsc_count].fd = subs.vicon_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- FLOW measurements --- */ - /* subscribe to ORB for flow measurements */ - subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); - fds[fdsc_count].fd = subs.flow_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- BATTERY STATUS --- */ - /* subscribe to ORB for flow measurements */ - subs.batt_sub = orb_subscribe(ORB_ID(battery_status)); - fds[fdsc_count].fd = subs.batt_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- DIFFERENTIAL PRESSURE --- */ - /* subscribe to ORB for flow measurements */ - subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - fds[fdsc_count].fd = subs.diff_pres_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- AIRSPEED --- */ - /* subscribe to ORB for airspeed */ - subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - fds[fdsc_count].fd = subs.airspeed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* WARNING: If you get the error message below, - * then the number of registered messages (fdsc) - * differs from the number of messages in the above list. - */ - if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - // const int timeout = 1000; - - thread_running = true; - - /* initialize log buffer with a size of 10 */ - sdlog_logbuffer_init(&lb, 10); - - /* initialize thread synchronization */ - pthread_mutex_init(&sysvector_mutex, NULL); - pthread_cond_init(&sysvector_cond, NULL); - - /* start logbuffer emptying thread */ - pthread_t sysvector_pthread = sysvector_write_start(&lb); - - starttime = hrt_absolute_time(); - - /* track skipping */ - int skip_count = 0; - - while (!thread_should_exit) { - - /* only poll for commands, gps and sensor_combined */ - int poll_ret = poll(fds, 3, 1000); - - /* handle the poll result */ - if (poll_ret == 0) { - /* XXX this means none of our providers is giving us data - might be an error? */ - } else if (poll_ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else { - - int ifds = 0; - - /* --- VEHICLE COMMAND VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy command into local buffer */ - orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - - /* always log to blackbox, even when logging disabled */ - blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, - buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, - (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); - - handle_command(&buf.cmd); - } - - /* --- VEHICLE GPS VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy gps position into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - - /* if logging disabled, continue */ - if (logging_enabled) { - - /* write KML line */ - } - } - - /* --- SENSORS RAW VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - // /* copy sensors raw data into local buffer */ - // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - // /* write out */ - // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); - - /* always copy sensors raw data into local buffer, since poll flags won't clear else */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres); - orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); - orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); - - /* if skipping is on or logging is disabled, ignore */ - if (skip_count < skip_value || !logging_enabled) { - skip_count++; - /* do not log data */ - continue; - } else { - /* log data, reset */ - skip_count = 0; - } - - struct sdlog_sysvector sysvect = { - .timestamp = buf.raw.timestamp, - .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, - .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, - .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, - .baro = buf.raw.baro_pres_mbar, - .baro_alt = buf.raw.baro_alt_meter, - .baro_temp = buf.raw.baro_temp_celcius, - .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, - .actuators = { - buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], - buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] - }, - .vbat = buf.batt.voltage_v, - .bat_current = buf.batt.current_a, - .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]}, - .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, - .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, - .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, - .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, - .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, - .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - .diff_pressure = buf.diff_pres.differential_pressure_pa, - .ind_airspeed = buf.airspeed.indicated_airspeed_m_s, - .true_airspeed = buf.airspeed.true_airspeed_m_s - }; - - /* put into buffer for later IO */ - pthread_mutex_lock(&sysvector_mutex); - sdlog_logbuffer_write(&lb, &sysvect); - /* signal the other thread new data, but not yet unlock */ - if ((unsigned)lb.count > (lb.size / 2)) { - /* only request write if several packets can be written at once */ - pthread_cond_signal(&sysvector_cond); - } - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&sysvector_mutex); - } - - } - - } - - print_sdlog_status(); - - /* wake up write thread one last time */ - pthread_mutex_lock(&sysvector_mutex); - pthread_cond_signal(&sysvector_cond); - /* unlock, now the writer thread may return */ - pthread_mutex_unlock(&sysvector_mutex); - - /* wait for write thread to return */ - (void)pthread_join(sysvector_pthread, NULL); - - pthread_mutex_destroy(&sysvector_mutex); - pthread_cond_destroy(&sysvector_cond); - - warnx("exiting.\n\n"); - - /* finish KML file */ - // XXX - fclose(gpsfile); - fclose(blackbox_file); - - thread_running = false; - - return 0; -} - -void print_sdlog_status() -{ - unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; - float mebibytes = bytes / 1024.0f / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; - - warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); -} - -/** - * @return 0 if file exists - */ -int file_exist(const char *filename) -{ - struct stat buffer; - return stat(filename, &buffer); -} - -int file_copy(const char *file_old, const char *file_new) -{ - FILE *source, *target; - source = fopen(file_old, "r"); - int ret = 0; - - if (source == NULL) { - warnx("failed opening input file to copy"); - return 1; - } - - target = fopen(file_new, "w"); - - if (target == NULL) { - fclose(source); - warnx("failed to open output file to copy"); - return 1; - } - - char buf[128]; - int nread; - - while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) { - int ret = fwrite(buf, 1, nread, target); - - if (ret <= 0) { - warnx("error writing file"); - ret = 1; - break; - } - } - - fsync(fileno(target)); - - fclose(source); - fclose(target); - - return ret; -} - -void handle_command(struct vehicle_command_s *cmd) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* request to set different system mode */ - switch (cmd->command) { - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param3)) == 1) { - - /* enable logging */ - mavlink_log_info(mavlink_fd, "[log] file:"); - mavlink_log_info(mavlink_fd, "logdir"); - logging_enabled = true; - } - if (((int)(cmd->param3)) == 0) { - - /* disable logging */ - mavlink_log_info(mavlink_fd, "[log] stopped."); - logging_enabled = false; - } - break; - - default: - /* silently ignore */ - break; - } - -} diff --git a/src/modules/sdlog/sdlog_ringbuffer.c b/src/modules/sdlog/sdlog_ringbuffer.c deleted file mode 100644 index d7c8a4759..000000000 --- a/src/modules/sdlog/sdlog_ringbuffer.c +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 sdlog_log.c - * MAVLink text logging. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <string.h> -#include <stdlib.h> - -#include "sdlog_ringbuffer.h" - -void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size) -{ - lb->size = size; - lb->start = 0; - lb->count = 0; - lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector)); -} - -int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb) -{ - return lb->count == (int)lb->size; -} - -int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb) -{ - return lb->count == 0; -} - - -// XXX make these functions thread-safe -void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem) -{ - int end = (lb->start + lb->count) % lb->size; - memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector)); - - if (sdlog_logbuffer_is_full(lb)) { - lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ - - } else { - ++lb->count; - } -} - -int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem) -{ - if (!sdlog_logbuffer_is_empty(lb)) { - memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector)); - lb->start = (lb->start + 1) % lb->size; - --lb->count; - return 0; - - } else { - return 1; - } -} diff --git a/src/modules/sdlog/sdlog_ringbuffer.h b/src/modules/sdlog/sdlog_ringbuffer.h deleted file mode 100644 index b65916459..000000000 --- a/src/modules/sdlog/sdlog_ringbuffer.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 sdlog_ringbuffer.h - * microSD logging - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#ifndef SDLOG_RINGBUFFER_H_ -#define SDLOG_RINGBUFFER_H_ - -#pragma pack(push, 1) -struct sdlog_sysvector { - uint64_t timestamp; /**< time [us] */ - float gyro[3]; /**< [rad/s] */ - float accel[3]; /**< [m/s^2] */ - float mag[3]; /**< [gauss] */ - float baro; /**< pressure [millibar] */ - float baro_alt; /**< altitude above MSL [meter] */ - float baro_temp; /**< [degree celcius] */ - float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ - float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */ - float vbat; /**< battery voltage in [volt] */ - float bat_current; /**< battery discharge current */ - float bat_discharged; /**< discharged energy in mAh */ - float adc[4]; /**< ADC ports [volt] */ - float local_position[3]; /**< tangent plane mapping into x,y,z [m] */ - int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */ - float attitude[3]; /**< roll, pitch, yaw [rad] */ - float rotMatrix[9]; /**< unitvectors */ - float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */ - float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ - float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */ - float diff_pressure; /**< differential pressure */ - float ind_airspeed; /**< indicated airspeed */ - float true_airspeed; /**< true airspeed */ -}; -#pragma pack(pop) - -struct sdlog_logbuffer { - unsigned int start; - // unsigned int end; - unsigned int size; - int count; - struct sdlog_sysvector *elems; -}; - -void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size); - -int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb); - -int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb); - -void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem); - -int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem); - -#endif diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6e..91230a37c 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bc49f5c85..07be3560a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -536,6 +536,20 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); /** + * Failsafe channel mapping. + * + * The RC mapping index indicates which channel is used for failsafe + * If 0, whichever channel is mapped to throttle is used + * otherwise the value indicates the specific rc channel to use + * + * @min 0 + * @max 18 + * + * + */ +PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function + +/** * Throttle control channel mapping. * * The channel index (starting from 1 for channel 1) indicates @@ -585,13 +599,13 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); /** - * Assist switch channel mapping. + * Posctl switch channel mapping. * * @min 0 * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -602,8 +616,6 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); -//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); - /** * Flaps channel mapping. * @@ -655,3 +667,83 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); + +/** + * Threshold for selecting assist mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); + +/** + * Threshold for selecting auto mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); + +/** + * Threshold for selecting posctl mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f); + +/** + * Threshold for selecting return to launch mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f); + +/** + * Threshold for selecting loiter mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e260aae45..18bf97f8d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -175,7 +175,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -219,8 +220,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ @@ -250,14 +251,13 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; - int rc_map_assisted_sw; + int rc_map_posctl_sw; int rc_map_loiter_sw; -// int rc_map_offboard_ctrl_mode_sw; - int rc_map_flaps; int rc_map_aux1; @@ -266,7 +266,17 @@ private: int rc_map_aux4; int rc_map_aux5; - int32_t rc_fs_thr; + int32_t rc_fails_thr; + float rc_assist_th; + float rc_auto_th; + float rc_posctl_th; + float rc_return_th; + float rc_loiter_th; + bool rc_assist_inv; + bool rc_auto_inv; + bool rc_posctl_inv; + bool rc_return_inv; + bool rc_loiter_inv; float battery_voltage_scaling; float battery_current_scaling; @@ -293,14 +303,13 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; - param_t rc_map_assisted_sw; + param_t rc_map_posctl_sw; param_t rc_map_loiter_sw; -// param_t rc_map_offboard_ctrl_mode_sw; - param_t rc_map_flaps; param_t rc_map_aux1; @@ -309,7 +318,12 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_fs_thr; + param_t rc_fails_thr; + param_t rc_assist_th; + param_t rc_auto_th; + param_t rc_posctl_th; + param_t rc_return_th; + param_t rc_loiter_th; param_t battery_voltage_scaling; param_t battery_current_scaling; @@ -416,7 +430,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace sensors @@ -499,6 +513,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); @@ -507,19 +522,22 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); + _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); -// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); - _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - /* RC failsafe */ - _parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR"); + /* RC thresholds */ + _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); + _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); + _parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); + _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); + _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); + _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -620,8 +638,9 @@ Sensors::parameters_update() } /* handle wrong values */ - if (!rc_valid) + if (!rc_valid) { warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + } const char *paramerr = "FAIL PARM LOAD"; @@ -642,6 +661,10 @@ Sensors::parameters_update() warnx("%s", paramerr); } + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx("%s", paramerr); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx("%s", paramerr); } @@ -650,7 +673,7 @@ Sensors::parameters_update() warnx("%s", paramerr); } - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) { warnx("%s", paramerr); } @@ -662,16 +685,27 @@ Sensors::parameters_update() warnx("%s", paramerr); } -// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { -// warnx("Failed getting offboard control mode sw chan 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_fs_thr, &(_parameters.rc_fs_thr)); + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); + param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); + _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); + _parameters.rc_assist_th = fabs(_parameters.rc_assist_th); + param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); + _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); + _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); + param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); + _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); + _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); + param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th)); + _parameters.rc_return_inv = (_parameters.rc_return_th < 0); + _parameters.rc_return_th = fabs(_parameters.rc_return_th); + param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); + _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); + _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -681,13 +715,11 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; + _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; -// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; @@ -800,12 +832,14 @@ Sensors::gyro_init() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ - if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) + if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) { ioctl(fd, GYROIOCSSAMPLERATE, 800); + } /* set the driver to poll at 1000Hz */ - if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) + if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) { ioctl(fd, SENSORIOCSPOLLRATE, 800); + } #else @@ -860,12 +894,15 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCGEXTERNAL, 0); - if (ret < 0) + if (ret < 0) { errx(1, "FATAL: unknown if magnetometer is external or onboard"); - else if (ret == 1) + + } else if (ret == 1) { _mag_is_external = true; - else + + } else { _mag_is_external = false; + } close(fd); } @@ -965,10 +1002,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - if (_mag_is_external) + if (_mag_is_external) { vect = _external_mag_rotation * vect; - else + + } else { vect = _board_rotation * vect; + } raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1086,8 +1125,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.gyro_scale[2], }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) { warn("WARNING: failed to set scale / offsets for gyro"); + } close(fd); @@ -1101,8 +1141,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.accel_scale[2], }; - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) { warn("WARNING: failed to set scale / offsets for accel"); + } close(fd); @@ -1116,8 +1157,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.mag_scale[2], }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) { warn("WARNING: failed to set scale / offsets for mag"); + } close(fd); @@ -1131,8 +1173,10 @@ Sensors::parameter_update_poll(bool forced) 1.0f, }; - if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) + if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + close(fd); } @@ -1150,10 +1194,12 @@ void Sensors::adc_poll(struct sensor_combined_s &raw) { /* only read if publishing */ - if (!_publishing) + if (!_publishing) { return; + } hrt_abstime t = hrt_absolute_time(); + /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ @@ -1178,6 +1224,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_v = voltage; + /* one-time initialization of low-pass value to avoid long init delays */ if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_filtered_v = voltage; @@ -1196,19 +1243,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* handle current only if voltage is valid */ if (_battery_status.voltage_v > 0.0f) { float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + /* check measured current value */ if (current >= 0.0f) { _battery_status.timestamp = t; _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { /* initialize discharged value */ - if (_battery_status.discharged_mah < 0.0f) + if (_battery_status.discharged_mah < 0.0f) { _battery_status.discharged_mah = 0.0f; + } + _battery_discharged += current * (t - _battery_current_timestamp); _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; } } } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1241,7 +1293,9 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } + _last_adc = t; + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { /* announce the battery status if needed, just publish else */ if (_battery_pub > 0) { @@ -1260,6 +1314,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max { if (_rc.function[func] >= 0) { float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { return min_value; @@ -1269,24 +1324,44 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } else { return value; } + } else { return 0.0f; } } switch_pos_t -Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { - float value = _rc.chan[_rc.function[func]].scaled; - if (value > STICK_ON_OFF_LIMIT) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; - } else if (value < -STICK_ON_OFF_LIMIT) { + } else if (mid_inv ? value < mid_th : value > mid_th) { + return SWITCH_POS_MIDDLE; + + } else { return SWITCH_POS_OFF; + } + + } else { + return SWITCH_POS_NONE; + } +} + +switch_pos_t +Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) +{ + if (_rc.function[func] >= 0) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return SWITCH_POS_ON; } else { - return SWITCH_POS_MIDDLE; + return SWITCH_POS_OFF; } } else { @@ -1318,13 +1393,18 @@ Sensors::rc_poll() /* signal looks good */ signal_lost = false; - /* check throttle failsafe */ - int8_t thr_ch = _rc.function[THROTTLE]; - if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { - /* throttle failsafe configured */ - if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { - /* throttle failsafe triggered, signal is lost by receiver */ + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle + + if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping + fs_ch = _parameters.rc_map_failsafe - 1; + } + + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { + /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } } @@ -1332,8 +1412,9 @@ Sensors::rc_poll() unsigned channel_limit = rc_input.channel_count; - if (channel_limit > _rc_max_chan_count) + if (channel_limit > _rc_max_chan_count) { channel_limit = _rc_max_chan_count; + } /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1341,11 +1422,13 @@ Sensors::rc_poll() /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (rc_input.values[i] < _parameters.min[i]) + if (rc_input.values[i] < _parameters.min[i]) { rc_input.values[i] = _parameters.min[i]; + } - if (rc_input.values[i] > _parameters.max[i]) + if (rc_input.values[i] > _parameters.max[i]) { rc_input.values[i] = _parameters.max[i]; + } /* * 2) Scale around the mid point differently for lower and upper range. @@ -1377,8 +1460,9 @@ Sensors::rc_poll() _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (!isfinite(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) { _rc.chan[i].scaled = 0.0f; + } } _rc.chan_count = rc_input.channel_count; @@ -1402,10 +1486,10 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.roll = get_rc_value(ROLL, -1.0, 1.0); - manual.pitch = get_rc_value(PITCH, -1.0, 1.0); - manual.yaw = get_rc_value(YAW, -1.0, 1.0); - manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual.y = get_rc_value(ROLL, -1.0, 1.0); + manual.x = get_rc_value(PITCH, -1.0, 1.0); + manual.r = get_rc_value(YAW, -1.0, 1.0); + manual.z = get_rc_value(THROTTLE, 0.0, 1.0); manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); @@ -1414,10 +1498,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); + manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { @@ -1433,10 +1517,10 @@ Sensors::rc_poll() actuator_group_3.timestamp = rc_input.timestamp_last_signal; - actuator_group_3.control[0] = manual.roll; - actuator_group_3.control[1] = manual.pitch; - actuator_group_3.control[2] = manual.yaw; - actuator_group_3.control[3] = manual.throttle; + actuator_group_3.control[0] = manual.y; + actuator_group_3.control[1] = manual.x; + actuator_group_3.control[2] = manual.r; + actuator_group_3.control[3] = manual.z; actuator_group_3.control[4] = manual.flaps; actuator_group_3.control[5] = manual.aux1; actuator_group_3.control[6] = manual.aux2; @@ -1560,8 +1644,9 @@ Sensors::task_main() diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ - if (_publishing) + if (_publishing) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + } /* Look for new r/c input data */ rc_poll(); @@ -1598,18 +1683,21 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: sensors {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (sensors::g_sensors != nullptr) + if (sensors::g_sensors != nullptr) { errx(0, "already running"); + } sensors::g_sensors = new Sensors; - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "alloc failed"); + } if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; @@ -1621,8 +1709,9 @@ int sensors_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "not running"); + } delete sensors::g_sensors; sensors::g_sensors = nullptr; @@ -1641,4 +1730,3 @@ int sensors_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; } - diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2d773fd25..7a499ca72 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -521,73 +521,15 @@ param_save_default(void) return ERROR; } - if (res == OK) { - res = param_export(fd, false); + res = param_export(fd, false); - if (res != OK) { - warnx("failed to write parameters to file: %s", filename); - } + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); } close(fd); return res; - -#if 0 - const char *filename_tmp = malloc(strlen(filename) + 5); - sprintf(filename_tmp, "%s.tmp", filename); - - /* delete temp file if exist */ - res = unlink(filename_tmp); - - if (res != OK && errno == ENOENT) - res = OK; - - if (res != OK) - warn("failed to delete temp file: %s", filename_tmp); - - if (res == OK) { - /* write parameters to temp file */ - fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) { - warn("failed to open temp file: %s", filename_tmp); - res = ERROR; - } - - if (res == OK) { - res = param_export(fd, false); - - if (res != OK) - warnx("failed to write parameters to file: %s", filename_tmp); - } - - close(fd); - } - - if (res == OK) { - /* delete parameters file */ - res = unlink(filename); - - if (res != OK && errno == ENOENT) - res = OK; - - if (res != OK) - warn("failed to delete parameters file: %s", filename); - } - - if (res == OK) { - /* rename temp file to parameters */ - res = rename(filename_tmp, filename); - - if (res != OK) - warn("failed to rename %s to %s", filename_tmp, filename); - } - - free(filename_tmp); - - return res; -#endif } /** diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 21e15ec56..c0c1a5cb4 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -140,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index a23d89cd2..19a29635b 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -64,22 +64,39 @@ struct manual_control_setpoint_s { /** * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. + * The variable names follow the definition of the + * MANUAL_CONTROL mavlink message. + * The default range is from -1 to 1 (mavlink message -1000 to 1000) + * The range for the z variable is defined from 0 to 1. (The z field of + * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) */ - float roll; /**< ailerons roll / roll rate input, -1..1 */ - float pitch; /**< elevator / pitch / pitch rate, -1..1 */ - float yaw; /**< rudder / yaw rate / yaw, -1..1 */ - float throttle; /**< throttle / collective thrust / altitude, 0..1 */ + float x; /**< stick position in x direction -1..1 + in general corresponds to forward/back motion or pitch of vehicle, + in general a positive value means forward or negative pitch and + a negative value means backward or positive pitch */ + float y; /**< stick position in y direction -1..1 + in general corresponds to right/left motion or roll of vehicle, + in general a positive value means right or positive roll and + a negative value means left or negative roll */ + float z; /**< throttle stick position 0..1 + in general corresponds to up/down motion or thrust of vehicle, + in general the value corresponds to the demanded throttle by the user, + if the input is used for setting the setpoint of a vertical position + controller any value > 0.5 means up and any value < 0.5 means down */ + float r; /**< yaw stick/twist positon, -1..1 + in general corresponds to the righthand rotation around the vertical + (downwards) axis of the vehicle */ float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ - switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ + switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ + switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ + switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index c168b2fac..b60402452 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,7 +64,7 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, - ASSISTED = 6, + POSCTL = 6, LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 435230432..f56355246 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,8 +63,8 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, - MAIN_STATE_SEATBELT, - MAIN_STATE_EASY, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, MAIN_STATE_AUTO, MAIN_STATE_MAX } main_state_t; |