dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware into ekf_params

This commit is contained in:
Lorenz Meier 2014-05-11 13:53:58 +02:00
parent da67b2b241 8f5c731b9e
commit dc19f16dee
18 changed files with 407 additions and 287 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/blinkm/blinkm.cpp
View File

@ -638,11 +638,11 @@ BlinkM::led()
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL)
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL)
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;

42
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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,12 +382,14 @@ 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 */
if (accel_disp[0] < still_thr2 &&
@ -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)
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)
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)
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)
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)
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)
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,9 +506,10 @@ 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++) {
accel_avg[i] = accel_sum[i] / count;
@ -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)
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++) {

6
src/modules/commander/airspeed_calibration.cpp
View File

@ -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 */

123
src/modules/commander/commander.cpp
View File

@ -233,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")) {
@ -261,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;
@ -304,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);
@ -371,8 +374,10 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed
// 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);
}
@ -417,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");
if (arming_res == TRANSITION_CHANGED)
if (arming_res == TRANSITION_CHANGED) {
ret = true;
}
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
@ -435,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_ALTCTRL) {
/* ALTCTRL */
main_res = main_state_transition(status, MAIN_STATE_ALTCTRL);
} 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_POSCTRL) {
/* POSCTRL */
main_res = main_state_transition(status, MAIN_STATE_POSCTRL);
} 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 */
@ -456,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) {
/* POSCTRL */
main_res = main_state_transition(status, MAIN_STATE_POSCTRL);
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
@ -466,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;
@ -484,16 +492,20 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
// 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 {
// 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;
}
@ -545,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) {
@ -588,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:
@ -601,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);
@ -634,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] = "ALTCTRL";
main_states_str[2] = "POSCTRL";
main_states_str[1] = "ALTCTL";
main_states_str[2] = "POSCTL";
main_states_str[3] = "AUTO";
char *arming_states_str[ARMING_STATE_MAX];
@ -883,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, &parachute_enabled);
@ -923,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");
}
@ -940,9 +957,11 @@ 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;
}
@ -950,10 +969,12 @@ int commander_thread_main(int argc, char *argv[])
} 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 */
@ -995,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;
@ -1008,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
@ -1077,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)
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;
@ -1320,6 +1344,7 @@ int commander_thread_main(int argc, char *argv[])
if (res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
} else if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
}
@ -1335,8 +1360,8 @@ int commander_thread_main(int argc, char *argv[])
}
// TODO remove this hack
/* flight termination in manual mode if assist switch is on posctrl position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_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);
}
@ -1350,9 +1375,10 @@ 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 */
bool arming_state_changed = check_arming_state_changed();
@ -1390,6 +1416,7 @@ int commander_thread_main(int argc, char *argv[])
status.condition_home_position_valid = true;
}
}
was_armed = armed.armed;
if (main_state_changed) {
@ -1553,21 +1580,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);
@ -1593,25 +1623,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break;
case SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctrl_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_POSCTRL);
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 ALTCTRL
print_reject_mode(status, "POSCTRL");
// else fallback to ALTCTL
print_reject_mode(status, "POSCTL");
}
res = main_state_transition(status, MAIN_STATE_ALTCTRL);
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (sp_man->posctrl_switch != SWITCH_POS_ON) {
print_reject_mode(status, "ALTCTRL");
if (sp_man->posctl_switch != SWITCH_POS_ON) {
print_reject_mode(status, "ALTCTL");
}
// else fallback to MANUAL
@ -1626,9 +1656,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 ALTCTRL (POSCTRL 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_ALTCTRL);
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@ -1674,7 +1704,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_ALTCTRL:
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;
@ -1685,7 +1715,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_POSCTRL:
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;
@ -1831,8 +1861,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) {
@ -1847,8 +1878,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) {
@ -1926,6 +1958,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) {
@ -1940,10 +1973,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);
@ -1963,11 +1998,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);
}
@ -1983,11 +2020,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);
}

20
src/modules/commander/commander_helper.cpp
View File

@ -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,23 +252,26 @@ 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;

12
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -320,15 +320,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
// MANUAL to ALTCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = true;
new_main_state = MAIN_STATE_ALTCTRL;
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("tranisition: manual to altctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state);
ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
// 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_ALTCTRL;
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("no transition: invalid local altitude",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
@ -336,15 +336,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
// MANUAL to POSCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = true;
new_main_state = MAIN_STATE_POSCTRL;
new_main_state = MAIN_STATE_POSCTL;
ut_assert("transition: manual to posctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state);
ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
// 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_POSCTRL;
new_main_state = MAIN_STATE_POSCTL;
ut_assert("no transition: invalid position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);

18
src/modules/commander/gyro_calibration.cpp
View File

@ -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;

31
src/modules/commander/mag_calibration.cpp
View File

@ -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);

4
src/modules/commander/px4_custom_mode.h
View File

@ -12,8 +12,8 @@
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_ALTCTRL,
PX4_CUSTOM_MAIN_MODE_POSCTRL,
PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
};

18
src/modules/commander/state_machine_helper.cpp
View File

@ -117,16 +117,19 @@ 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) {
@ -137,8 +140,10 @@ arming_state_transition(struct vehicle_status_s *status, /// current
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;
}
@ -169,9 +174,11 @@ arming_state_transition(struct vehicle_status_s *status, /// current
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]);
}
@ -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_ALTCTRL:
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_POSCTRL:
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;

4
src/modules/fixedwing_backside/fixedwing.cpp
View File

@ -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_ALTCTRL ||
_status.main_state == MAIN_STATE_POSCTRL /* TODO, implement pos control */) {
} 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

8
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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_ALTCTRL) {
} 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_ALTCTRL;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
} else if (status->main_state == MAIN_STATE_POSCTRL) {
} 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_POSCTRL;
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;

4
src/modules/segway/BlockSegwayController.cpp
View File

@ -35,8 +35,8 @@ void BlockSegwayController::update() {
// handle autopilot modes
if (_status.main_state == MAIN_STATE_AUTO ||
_status.main_state == MAIN_STATE_ALTCTRL ||
_status.main_state == MAIN_STATE_POSCTRL) {
_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;

8
src/modules/sensors/sensor_params.c
View File

@ -599,7 +599,7 @@ 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
@ -616,8 +616,6 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
/**
* Flaps channel mapping.
*
@ -703,7 +701,7 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
/**
* Threshold for selecting posctrl mode
* Threshold for selecting posctl mode
*
* min:-1
* max:+1
@ -716,7 +714,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
* negative : true when channel<th
*
*/
PARAM_DEFINE_FLOAT(RC_POSCTRL_TH, 0.5f);
PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f);
/**
* Threshold for selecting return to launch mode

122
src/modules/sensors/sensors.cpp
View File

@ -255,11 +255,9 @@ private:
int rc_map_mode_sw;
int rc_map_return_sw;
int rc_map_posctrl_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;
@ -271,12 +269,12 @@ private:
int32_t rc_fails_thr;
float rc_assist_th;
float rc_auto_th;
float rc_posctrl_th;
float rc_posctl_th;
float rc_return_th;
float rc_loiter_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_posctrl_inv;
bool rc_posctl_inv;
bool rc_return_inv;
bool rc_loiter_inv;
@ -309,11 +307,9 @@ private:
param_t rc_map_mode_sw;
param_t rc_map_return_sw;
param_t rc_map_posctrl_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;
@ -325,7 +321,7 @@ private:
param_t rc_fails_thr;
param_t rc_assist_th;
param_t rc_auto_th;
param_t rc_posctrl_th;
param_t rc_posctl_th;
param_t rc_return_th;
param_t rc_loiter_th;
@ -526,11 +522,9 @@ Sensors::Sensors() :
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_posctrl_sw = param_find("RC_MAP_POSCTL_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");
@ -541,7 +535,7 @@ Sensors::Sensors() :
_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_posctrl_th = param_find("RC_POSCTRL_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");
@ -644,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";
@ -678,7 +673,7 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_posctrl_sw, &(_parameters.rc_map_posctrl_sw)) != OK) {
if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
warnx("%s", paramerr);
}
@ -690,10 +685,6 @@ 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));
@ -706,9 +697,9 @@ Sensors::parameters_update()
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_posctrl_th, &(_parameters.rc_posctrl_th));
_parameters.rc_posctrl_inv = (_parameters.rc_posctrl_th<0);
_parameters.rc_posctrl_th = fabs(_parameters.rc_posctrl_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);
@ -724,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[POSCTRL] = _parameters.rc_map_posctrl_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;
@ -843,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
@ -903,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);
}
@ -1008,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);
@ -1129,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);
@ -1144,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);
@ -1159,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);
@ -1174,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);
}
@ -1193,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) */
@ -1221,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;
@ -1239,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) {
@ -1284,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) {
@ -1303,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;
@ -1312,6 +1324,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
} else {
return value;
}
} else {
return 0.0f;
}
@ -1322,6 +1335,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo
{
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;
@ -1342,6 +1356,7 @@ Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo
{
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;
@ -1380,9 +1395,11 @@ Sensors::rc_poll()
/* 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) ||
@ -1395,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++) {
@ -1404,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.
@ -1440,9 +1460,10 @@ 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;
_rc.rssi = rc_input.rssi;
@ -1478,7 +1499,7 @@ Sensors::rc_poll()
/* mode switches */
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.posctrl_switch = get_rc_sw2pos_position(POSCTRL, _parameters.rc_posctrl_th, _parameters.rc_posctrl_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);
@ -1623,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();
@ -1661,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;
@ -1684,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;

4
src/modules/uORB/topics/manual_control_setpoint.h
View File

@ -77,8 +77,8 @@ struct manual_control_setpoint_s {
float aux5; /**< default function: payload drop */
switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): no effect, return */
switch_pos_t posctrl_switch; /**< posctrl 2 position switch (optional): altctrl, posctrl */
switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): normal, return */
switch_pos_t posctl_switch; /**< posctrl 2 position switch (optional): altctl, posctl */
switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */
}; /**< manual control inputs */

2
src/modules/uORB/topics/rc_channels.h
View File

@ -64,7 +64,7 @@ enum RC_CHANNELS_FUNCTION {
YAW = 3,
MODE = 4,
RETURN = 5,
POSCTRL = 6,
POSCTL = 6,
LOITER = 7,
OFFBOARD_MODE = 8,
FLAPS = 9,

4
src/modules/uORB/topics/vehicle_status.h
View File

@ -63,8 +63,8 @@
/* main state machine */
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ALTCTRL,
MAIN_STATE_POSCTRL,
MAIN_STATE_ALTCTL,
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO,
MAIN_STATE_MAX
} main_state_t;