diff options
author | philipoe <philipp.oettershagen@mavt.ethz.ch> | 2014-11-20 17:26:27 +0100 |
---|---|---|
committer | philipoe <philipp.oettershagen@mavt.ethz.ch> | 2014-11-20 17:26:27 +0100 |
commit | ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f (patch) | |
tree | b5c5f7d4781281a8209a5580a5bb3e1268a7e6b5 /src/modules | |
parent | 5c34f03c4ef1d0b928c8ad32f8038bb15dba7c11 (diff) | |
parent | 97a1410ec99e880207e4ee6d2a03451c2e11f4cf (diff) | |
download | px4-firmware-ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f.tar.gz px4-firmware-ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f.tar.bz2 px4-firmware-ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f.zip |
Merge remote-tracking branch 'upstream/master' into PR_RCLossDur2
Diffstat (limited to 'src/modules')
47 files changed, 988 insertions, 323 deletions
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 31c9157e1..e0bcbc6e9 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -223,7 +223,7 @@ BottleDrop::start() _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, - 2048, + 1500, (main_t)&BottleDrop::task_main_trampoline, nullptr); @@ -283,7 +283,6 @@ BottleDrop::drop() // force the door open if we have to if (_doors_opened == 0) { open_bay(); - warnx("bay not ready, forced open"); } while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { @@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { open_bay(); drop(); - mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + mavlink_log_critical(_mavlink_fd, "drop bottle"); } else if (cmd->param1 > 0.5f) { open_bay(); - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + mavlink_log_critical(_mavlink_fd, "opening bay"); } else { lock_release(); close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + mavlink_log_critical(_mavlink_fd, "closing bay"); } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: _drop_approval = false; - mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + mavlink_log_critical(_mavlink_fd, "got drop position, no approval"); break; case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + mavlink_log_critical(_mavlink_fd, "got drop position and approval"); break; default: @@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command); break; default: diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 6b18fff55..df9f5473b 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -39,3 +39,7 @@ MODULE_COMMAND = bottle_drop SRCS = bottle_drop.cpp \ bottle_drop_params.c + +MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a4e7a766..0cb41489f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -83,7 +83,7 @@ * | accel_T[1][i] | * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough * | accel_corr_ref[2][i] | * [ accel_corr_ref[4][i] ] * @@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "You need to put the system on all six sides"); + sleep(3); + mavlink_log_info(mavlink_fd, "Follow the instructions on the screen"); + sleep(5); + struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; } - mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + /* inform user which axes are still needed */ + mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[0]) ? "front " : "", + (!data_collected[1]) ? "back " : "", + (!data_collected[2]) ? "left " : "", + (!data_collected[3]) ? "right " : "", + (!data_collected[4]) ? "up " : "", + (!data_collected[5]) ? "down " : ""); + + /* allow user enough time to read the message */ + sleep(3); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - res = ERROR; - break; + mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); + sleep(3); + continue; } + /* inform user about already handled side */ if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + sleep(4); continue; } - mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); + sleep(1); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); + mavlink_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); t_still = 0; } } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 339b11bbe..cae1d7684 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -61,6 +61,15 @@ static const int ERROR = -1; static const char *sensor_name = "dpress"; +#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd" + +static void feedback_calibration_failed(int mavlink_fd) +{ + sleep(5); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG); +} + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ @@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); close(diff_pres_sub); return ERROR; } @@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd) } } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); return ERROR; } else { mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", @@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1f13aaec4..deb048566 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -207,7 +207,9 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -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); +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); /** * Mainloop of commander. @@ -218,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); - transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -230,7 +230,8 @@ void print_reject_arm(const char *msg); 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 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); @@ -260,7 +261,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2950, + 3200, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -393,7 +394,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // 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, true /* fRunPreArmChecks */, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -406,11 +408,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char } bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, - struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) + && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -537,13 +540,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", - (double)cmd->param1, - (double)cmd->param2, - (double)cmd->param3, - (double)cmd->param4, - (double)cmd->param5, - (double)cmd->param6, - (double)cmd->param7); + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -554,35 +557,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s //XXX update state machine? armed_local->force_failsafe = true; warnx("forcing failsafe (termination)"); + } else { armed_local->force_failsafe = false; warnx("disabling failsafe (termination)"); } + /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; status_local->data_link_lost_cmd = false; status_local->gps_failure_cmd = false; status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ warnx("reset all non-flighttermination failsafe commands"); + } else if ((int)cmd->param2 == 1) { /* trigger engine failure mode */ status_local->engine_failure_cmd = true; warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { /* trigger data link loss mode */ status_local->data_link_lost_cmd = true; warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { /* trigger gps loss mode */ status_local->gps_failure_cmd = true; warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { /* trigger rc loss mode */ status_local->rc_signal_lost_cmd = true; warnx("rc loss mode commanded"); } + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; @@ -633,21 +644,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } + if (cmd->param1 > 0.5f) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); status_local->offboard_control_set_by_command = false; + } else { /* Set flag that offboard was set via command, main state is not overridden by rc */ status_local->offboard_control_set_by_command = true; } + } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ @@ -656,6 +673,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -740,7 +758,10 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -802,6 +823,7 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); @@ -827,11 +849,14 @@ int commander_thread_main(int argc, char *argv[]) /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = -1; mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, + mission.current_seq); mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", - mission.dataman_id, mission.count, mission.current_seq); + mission.dataman_id, mission.count, mission.current_seq); + } else { const char *missionfail = "reading mission state failed"; warnx("%s", missionfail); @@ -1097,6 +1122,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; status_changed = true; } + } else { if (!status.offboard_control_signal_lost) { status.offboard_control_signal_lost = true; @@ -1115,9 +1141,9 @@ int commander_thread_main(int argc, char *argv[]) /* perform system checks when new telemetry link connected */ if (mavlink_fd && - telemetry_last_heartbeat[i] == 0 && - telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1130,6 +1156,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. * Because the barometer is used for calculating AMSL altitude which is used to ensure * vertical separation from other airtraffic the operator has to know when the @@ -1142,6 +1169,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; mavlink_log_critical(mavlink_fd, "baro healthy"); } + } else { if (!status.barometer_failure) { status.barometer_failure = true; @@ -1164,10 +1192,11 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&system_power.timestamp) < 200000) { if (system_power.servo_valid && - !system_power.brick_valid && - !system_power.usb_connected) { + !system_power.brick_valid && + !system_power.usb_connected) { /* flying only on servo rail, this is unsafe */ status.condition_power_input_valid = false; + } else { status.condition_power_input_valid = true; } @@ -1187,9 +1216,11 @@ 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); + 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, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1233,7 +1264,8 @@ int commander_thread_main(int argc, char *argv[]) } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), + &status_changed); /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && @@ -1283,8 +1315,11 @@ int commander_thread_main(int argc, char *argv[]) local_eph_good = false; } } - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid + && local_eph_good, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, + &(status.condition_local_altitude_valid), &status_changed); if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { @@ -1315,7 +1350,8 @@ int commander_thread_main(int argc, char *argv[]) /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, + throttle); } } @@ -1372,8 +1408,8 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - //struct stat statbuf; - //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + struct stat statbuf; + on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1383,26 +1419,30 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } + status_changed = true; } @@ -1411,7 +1451,8 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1439,23 +1480,25 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_threshold) - && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ - globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } /* check if GPS fix is ok */ if (status.circuit_breaker_engaged_gpsfailure_check || - (gps_position.fix_type >= 3 && - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; status_changed = true; mavlink_log_critical(mavlink_fd, "gps regained"); } + } else { if (!status.gps_failure) { status.gps_failure = true; @@ -1477,12 +1520,14 @@ int commander_thread_main(int argc, char *argv[]) armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination @@ -1490,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[]) /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && - hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1515,11 +1560,15 @@ int commander_thread_main(int argc, char *argv[]) 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); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } + stick_off_counter = 0; } else { @@ -1541,8 +1590,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1565,6 +1617,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "DISARMED by RC"); } + arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { @@ -1601,18 +1654,20 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && - hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { /* handle the case where data link was regained, * accept datalink as healthy only after datalink_regain_timeout seconds * */ if (telemetry_lost[i] && - hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration * we don't have to check a timeout */ @@ -1621,6 +1676,7 @@ int commander_thread_main(int argc, char *argv[]) } else { telemetry_last_dl_loss[i] = hrt_absolute_time(); + if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; @@ -1648,24 +1704,26 @@ int commander_thread_main(int argc, char *argv[]) * only for fixed wing for now */ if (!status.circuit_breaker_engaged_enginefailure_check && - status.is_rotary_wing == false && - armed.armed && - ((actuator_controls.control[3] > ef_throttle_thres && - battery.current_a/actuator_controls.control[3] < - ef_current2throttle_thres) || - (status.engine_failure))) { + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a / actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { /* potential failure, measure time */ if (timestamp_engine_healthy > 0 && - hrt_elapsed_time(×tamp_engine_healthy) > - ef_time_thres * 1e6 && - !status.engine_failure) { - status.engine_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "Engine Failure"); + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); } + } else { /* no failure reset flag */ timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { status.engine_failure = false; status_changed = true; @@ -1692,20 +1750,22 @@ int commander_thread_main(int argc, char *argv[]) * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && - ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd))) { + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } } @@ -1714,19 +1774,21 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.gps_failure) || - (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } @@ -1767,6 +1829,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } @@ -1787,7 +1850,11 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + if (status.failsafe) { + mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { + mavlink_log_critical(mavlink_fd, "failsafe mode off"); + } failsafe_old = status.failsafe; } @@ -1811,7 +1878,8 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available + && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; @@ -1989,6 +2057,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2010,6 +2079,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } else { res = main_state_transition(status_local, MAIN_STATE_MANUAL); } + // TRANSITION_DENIED is not possible here break; @@ -2024,7 +2094,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "POSCTL"); } - // fallback to ALTCTL + // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -2048,14 +2118,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); @@ -2064,7 +2134,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); @@ -2073,22 +2143,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } - // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + // fallback to POSCTL + res = main_state_transition(status_local, MAIN_STATE_POSCTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); @@ -2132,18 +2202,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_termination_enabled = false; - break; - case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2156,59 +2214,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_OFFBOARD: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_offboard_enabled = true; - - switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_force_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - //XXX: the flags could depend on sp_offboard.ignore - break; - default: - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - } - break; - case NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2224,6 +2229,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: case NAVIGATION_STATE_AUTO_RTGS: case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; @@ -2249,6 +2255,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2262,6 +2281,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_DESCEND: + /* TODO: check if this makes sense */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; @@ -2275,6 +2307,63 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; + case NAVIGATION_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = true; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + //XXX: the flags could depend on sp_offboard.ignore + break; + + default: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + break; + default: break; } @@ -2416,7 +2505,8 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d89c67c2b..8ab14dd52 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -63,7 +63,10 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); struct gyro_scale gyro_scale = { 0.0f, diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 23900f386..7be8de9c6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9568752ae..e37019d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, * Check failsafe and main status and set navigation status for navigator accordingly */ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; @@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_MISSION: + /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if either the datalink is enabled and lost as well as RC is lost - * - if there is no datalink and the mission is finished */ + * - depending on datalink, RC and if the mission is finished */ + + /* first look at the commands */ if (status->engine_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { @@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->gps_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX - /* Finished handling commands which have priority , now handle failures */ + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || - (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { + + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost */ - } else if (status->data_link_lost && data_link_loss_enabled) { + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost && !stay_in_failsafe) { - - /* this mode is ok, we don't need RC for missions */ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ - /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index f0139a4b7..2860d1efc 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -39,3 +39,5 @@ SRCS = test_params.c \ block/BlockParam.cpp \ uorb/blocks.cpp \ blocks.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 685f5e12f..e7805daa9 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state() warn_index = max_warn_index; } - warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + // Do not warn about accel offset if we have no position updates + if (!(warn_index == 5 && _ekf->staticMode)) { + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + } } struct estimator_status_report rep; @@ -1557,7 +1560,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 5000, + 7500, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bec706bad..9afe74af1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, - 50, - MAV_TYPE_FIXED_WING, - 0, - 0, - 0 + 50 }; // System ID, 1-255, Component/Subsystem ID, 1-255 /* diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d932d20a..fb9f65cf5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,8 +167,10 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), - _param_system_type(0), + _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void) param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; + _system_type = system_type; } int32_t use_hil_gps; @@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -763,7 +765,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f96e586b..ad5e5001b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -265,6 +265,8 @@ public: struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + unsigned get_system_type() { return _system_type; } + protected: Mavlink *next; @@ -354,6 +356,8 @@ private: param_t _param_use_hil_gps; param_t _param_forward_externalsp; + unsigned _system_type; + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf..978aee118 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -302,7 +302,7 @@ protected: msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); - msg.type = mavlink_system.type; + msg.type = _mavlink->get_system_type(); msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; @@ -382,11 +382,11 @@ protected: clock_gettime(CLOCK_REALTIME, &ts); /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); + struct tm tt; + gmtime_r(&gps_time_sec, &tt); // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); } @@ -1353,15 +1353,17 @@ protected: const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + unsigned system_type = _mavlink->get_system_type(); + /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; - switch (mavlink_system.type) { + switch (system_type) { case MAV_TYPE_QUADROTOR: n = 4; break; @@ -1717,7 +1719,53 @@ protected: msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - msg.rssi = rc.rssi; + + /* RSSI has a max value of 100, and when Spektrum or S.BUS are + * available, the RSSI field is invalid, as they do not provide + * an RSSI measurement. Use an out of band magic value to signal + * these digital ports. XXX revise MAVLink spec to address this. + * One option would be to use the top bit to toggle between RSSI + * and input source mode. + * + * Full RSSI field: 0b 1 111 1111 + * + * ^ If bit is set, RSSI encodes type + RSSI + * + * ^ These three bits encode a total of 8 + * digital RC input types. + * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 + * ^ These four bits encode a total of + * 16 RSSI levels. 15 = full, 0 = no signal + * + */ + + /* Initialize RSSI with the special mode level flag */ + msg.rssi = (1 << 7); + + /* Set RSSI */ + msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; + + switch (rc.input_source) { + case RC_INPUT_SOURCE_PX4FMU_PPM: + /* fallthrough */ + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; + } + + if (rc.rc_lost) { + /* RSSI is by definition zero */ + msg.rssi = 0; + } _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e8d783847..bc092c7e9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -391,11 +391,9 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) f.timestamp = hrt_absolute_time(); f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.flow_raw_x = flow.integrated_x; + f.flow_raw_y = flow.integrated_y; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; @@ -413,7 +411,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) r.timestamp = hrt_absolute_time(); r.error_count = 0; r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.ground_distance; + r.distance = flow.distance; r.minimum_distance = 0.0f; r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); @@ -1398,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 40; + param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 66f1c8c73..e789fd10d 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item() /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -188,6 +189,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { if (!_param_skipcommshold.get()) { @@ -208,6 +210,7 @@ DataLinkLoss::advance_dll() _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; @@ -215,6 +218,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: warnx("dll end"); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a61..0765e9b7c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -371,7 +371,7 @@ Mission::set_mission_items() } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - return false; - } - - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after + * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ @@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, + &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_critical(_navigator->get_mavlink_fd(), - "DO JUMP repetitions completed"); + if (is_current) { + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); + } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } @@ -707,6 +712,7 @@ Mission::set_mission_item_reached() _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; _navigator->publish_mission_result(); + reset_mission_item_reached(); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a867dd0da..10a4ee88f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -289,8 +289,9 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position updates to 50 Hz */ + /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -497,8 +498,8 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, + SCHED_PRIORITY_DEFAULT + 20, + 1800, (main_t)&Navigator::task_main_trampoline, nullptr); @@ -630,9 +631,6 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } void diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 5564a1c42..42392e739 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -163,6 +163,7 @@ RCLoss::advance_rcl() _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); } break; case RCL_STATE_LOITER: @@ -171,6 +172,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: warnx("rcl end"); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 7b127759a..58c9429b6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -41,6 +41,7 @@ #include <stdbool.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_rc_input.h> #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> #include <rc/st24.h> @@ -60,6 +61,8 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; +static uint16_t rc_value_override = 0; + bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); @@ -68,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); if (*dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; @@ -89,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } @@ -168,6 +171,12 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + if (dsm_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + } + if (st24_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -316,6 +325,9 @@ controls_tick() { r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); + } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { + /* pick out override channel, indicated by special mapping */ + rc_value_override = SIGNED_TO_REG(scaled); } } } @@ -409,9 +421,18 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; + /* + if the FMU is dead then enable override if we have a + mixer and OVERRIDE_IMMEDIATE is set + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + override = true; + if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 3e19333d8..c0b8ac358 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -60,13 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 500000 -/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ -#define ROLL 0 -#define PITCH 1 -#define YAW 2 -#define THROTTLE 3 -#define OVERRIDE 4 - /* current servo arm/disarm state */ static bool mixer_servos_armed = false; static bool should_arm = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 89a470b44..c7e9ae3eb 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,7 @@ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ @@ -221,6 +222,7 @@ enum { /* DSM bind states */ hence index 12 can safely be used. */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ /* autopilot control values, -10000..10000 */ @@ -246,6 +248,7 @@ enum { /* DSM bind states */ #define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ #define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ #define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ #define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd134ccb4..14ee9cb40 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in; #define NUM_MSG 2 static char msg[NUM_MSG][40]; +static void heartbeat_blink(void); +static void ring_blink(void); + /* * add a debug message to be printed on the console */ @@ -126,6 +129,65 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static void +ring_blink(void) +{ +#ifdef GPIO_LED4 + + if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + LED_RING(1); + return; + } + + // XXX this led code does have + // intentionally a few magic numbers. + const unsigned max_brightness = 118; + + static unsigned counter = 0; + static unsigned brightness = max_brightness; + static unsigned brightness_counter = 0; + static unsigned on_counter = 0; + + if (brightness_counter < max_brightness) { + + bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); + + // XXX once led is PWM driven, + // remove the ! in the line below + // to return to the proper breathe + // animation / pattern (currently inverted) + LED_RING(!on); + brightness_counter++; + + if (on) { + on_counter++; + } + + } else { + + if (counter >= 62) { + counter = 0; + } + + int n; + + if (counter < 32) { + n = counter; + + } else { + n = 62 - counter; + } + + brightness = (n * n) / 8; + brightness_counter = 0; + on_counter = 0; + counter++; + } + +#endif +} + static uint64_t reboot_time; /** @@ -191,6 +253,9 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); +#ifdef GPIO_LED4 + LED_RING(false); +#endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO @@ -294,6 +359,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + ring_blink(); + check_reboot(); /* check for debug activity (default: none) */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index e32fcc72b..93a33490f 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7a5a5e484..fbfdd35db 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -603,6 +604,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; + case PX4IO_P_SETUP_FORCE_SAFETY_ON: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } + break; + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; @@ -686,7 +693,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { count++; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0f0414148..d76ec55f0 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -57,11 +57,12 @@ #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 #define SBUS_FRAMELOST_BIT 2 +#define SBUS1_FRAME_DELAY 14000 /* Measured values with Futaba FX-30/R6108SB: -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) - -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) */ @@ -80,6 +81,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; +static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; @@ -87,13 +89,15 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) { - if (sbus_fd < 0) + if (sbus_fd < 0) { sbus_fd = open(device, O_RDWR | O_NONBLOCK); + } if (sbus_fd >= 0) { struct termios t; @@ -113,16 +117,49 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } + return sbus_fd; } void sbus1_output(uint16_t *values, uint16_t num_values) { - char a = 'A'; - write(sbus_fd, &a, 1); -} + uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ + uint8_t offset = 0; + uint16_t value; + hrt_abstime now; + + now = hrt_absolute_time(); + + if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { + last_txframe_time = now; + uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f }; + + /* 16 is sbus number of servos/channels minus 2 single bit channels. + * currently ignoring single bit channels. */ + for (unsigned i = 0; (i < num_values) && (i < 16); ++i) { + value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); + + /*protect from out of bounds values and limit to 11 bits*/ + if (value > 0x07ff ) { + value = 0x07ff; + } + + while (offset >= 8) { + ++byteindex; + offset -= 8; + } + + oframe[byteindex] |= (value << (offset)) & 0xff; + oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff; + oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff; + offset += 11; + } + + write(sbus_fd, oframe, SBUS_FRAME_SIZE); + } +} void sbus2_output(uint16_t *values, uint16_t num_values) { @@ -167,8 +204,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ - if (ret < 1) + if (ret < 1) { return false; + } last_rx_time = now; @@ -180,8 +218,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb /* * If we don't have a full frame, return */ - if (partial_frame_count < SBUS_FRAME_SIZE) + if (partial_frame_count < SBUS_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a frame. Go ahead and @@ -228,7 +267,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -237,23 +277,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } switch (frame[24]) { - case 0x00: + case 0x00: /* this is S.BUS 1 */ break; - case 0x03: + + case 0x03: /* S.BUS 2 SLOT0: RX battery and external voltage */ break; - case 0x83: + + case 0x83: /* S.BUS 2 SLOT1 */ break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: + + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: break; - default: + + default: /* we expect one of the bits above, but there are some we don't know yet */ break; } @@ -283,7 +327,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ @@ -304,16 +348,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - } - else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + + } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ *sbus_failsafe = false; *sbus_frame_drop = true; + } else { *sbus_failsafe = false; *sbus_frame_drop = false; diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index a28d43e72..d4a00af39 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -43,3 +43,5 @@ SRCS = sdlog2.c \ logbuffer.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1282bc0ea..af580f1f7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include <uORB/topics/system_power.h> #include <uORB/topics/servorail_status.h> #include <uORB/topics/wind_estimate.h> +#include <uORB/topics/encoders.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -954,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct servorail_status_s servorail_status; struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; + struct encoders_s encoders; } buf; memset(&buf, 0, sizeof(buf)); @@ -996,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; + struct log_ENCD_s log_ENCD; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1033,12 +1036,12 @@ int sdlog2_thread_main(int argc, char *argv[]) int system_power_sub; int servorail_status_sub; int wind_sub; + int encoders_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1057,9 +1060,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); - } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1068,6 +1068,25 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); + subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); + + /* add new topics HERE */ + + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } + + if (_extended_logging) { + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); + } + + /* close non-needed fd's */ + + /* close stdin */ + close(0); + /* close stdout */ + close(1); thread_running = true; @@ -1652,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(WIND); } + /* --- ENCODERS --- */ + if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + log_msg.msg_type = LOG_ENCD_MSG; + log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; + log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1]; + log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCD); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d49fc0c79..fa9bdacb8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -407,6 +407,16 @@ struct log_VISN_s { float qw; }; +/* --- ENCODERS - ENCODER DATA --- */ +#define LOG_ENCD_MSG 39 +struct log_ENCD_s { + int64_t cnt0; + float vel0; + int64_t cnt1; + float vel1; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), + LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index 5b1bc5e86..dfbba92d9 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -42,3 +42,5 @@ SRCS = sensors.cpp \ sensor_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c new file mode 100644 index 000000000..4bcf95784 --- /dev/null +++ b/src/modules/systemlib/mcu_version.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mcu_version.c + * + * Read out the microcontroller version from the board + * + * @author Lorenz Meier <lorenz@px4.io> + * + */ + +#include "mcu_version.h" + +#include <nuttx/config.h> + +#ifdef CONFIG_ARCH_CHIP_STM32 +#include <up_arch.h> + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +#endif + + + +int mcu_version(char* rev, char** revstr) +{ +#ifdef CONFIG_ARCH_CHIP_STM32 + uint32_t abc = getreg32(DBGMCU_IDCODE); + + int32_t chip_version = abc & DEVID_MASK; + enum MCU_REV revid = (abc & REVID_MASK) >> 16; + + switch (chip_version) { + case STM32F40x_41x: + *revstr = "STM32F40x"; + break; + case STM32F42x_43x: + *revstr = "STM32F42x"; + break; + default: + *revstr = "STM32F???"; + break; + } + + switch (revid) { + + case MCU_REV_STM32F4_REV_A: + *rev = 'A'; + break; + case MCU_REV_STM32F4_REV_Z: + *rev = 'Z'; + break; + case MCU_REV_STM32F4_REV_Y: + *rev = 'Y'; + break; + case MCU_REV_STM32F4_REV_1: + *rev = '1'; + break; + case MCU_REV_STM32F4_REV_3: + *rev = '3'; + break; + default: + *rev = '?'; + revid = -1; + break; + } + + return revid; +#else + return -1; +#endif +} diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h new file mode 100644 index 000000000..1b3d0aba9 --- /dev/null +++ b/src/modules/systemlib/mcu_version.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +#pragma once + +/* magic numbers from reference manual */ +enum MCU_REV { + MCU_REV_STM32F4_REV_A = 0x1000, + MCU_REV_STM32F4_REV_Z = 0x1001, + MCU_REV_STM32F4_REV_Y = 0x1003, + MCU_REV_STM32F4_REV_1 = 0x1007, + MCU_REV_STM32F4_REV_3 = 0x2001 +}; + +/** + * Reports the microcontroller version of the main CPU. + * + * @param rev The silicon revision character + * @param revstr The full chip name string + * @return The silicon revision / version number as integer + */ +__EXPORT int mcu_version(char* rev, char** revstr); diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index bf3428a50..0d629d610 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { warnx("line too long"); + fclose(fp); return -1; } @@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) strcat(buf, line); } + fclose(fp); return 0; } diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa0..fe8b7e75a 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,5 +53,7 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c + circuit_breaker.c \ + mcu_version.c +MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 0c29101fe..9385ce253 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -44,3 +44,5 @@ SRCS = uORB.cpp \ objects_common.cpp \ Publication.cpp \ Subscription.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b921865eb..b91a00c1e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -201,6 +201,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); +#include "topics/test_motor.h" +ORB_DEFINE(test_motor, struct test_motor_s); + #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index dde237adc..50b7bd9e5 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -47,7 +47,7 @@ * Switch position */ typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_NONE = 0, /**< switch is not mapped */ SWITCH_POS_ON, /**< switch activated (value = 1) */ SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ SWITCH_POS_OFF /**< switch not activated (value = -1) */ @@ -93,13 +93,13 @@ struct manual_control_setpoint_s { float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ - 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 */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; /**< manual control inputs */ + 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 */ + switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ + switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ +}; /** * @} diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 8978de471..16916cc4d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -34,6 +34,8 @@ /** * @file rc_channels.h * Definition of the rc_channels uORB topic. + * + * @deprecated DO NOT USE FOR NEW CODE */ #ifndef RC_CHANNELS_H_ @@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION { AUX_2, AUX_3, AUX_4, - AUX_5, - RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */ + AUX_5 }; +// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS + +#define RC_CHANNELS_FUNCTION_MAX 18 + /** * @addtogroup topics * @{ @@ -76,7 +81,6 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ uint8_t channel_count; /**< Number of valid channels */ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */ int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ uint8_t rssi; /**< Receive signal strength index */ bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h new file mode 100644 index 000000000..2dd90e69d --- /dev/null +++ b/src/modules/uORB/topics/test_motor.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_motor.h + * + * Test a single drive motor while the vehicle is disarmed + * + * Publishing values to this topic causes a single motor to spin, e.g. for + * ground test purposes. This topic will be ignored while the vehicle is armed. + * + */ + +#ifndef TOPIC_TEST_MOTOR_H +#define TOPIC_TEST_MOTOR_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_MOTOR_OUTPUTS 8 + +/** + * @addtogroup topics + * @{ + */ + +struct test_motor_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + unsigned motor_number; /**< number of motor to spin */ + float value; /**< output power, range [0..1] */ +}; + +/** + * @} + */ + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(test_motor); + +#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 8d797e21a..8ec9d98d6 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -108,7 +108,7 @@ typedef enum { NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ - NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ NAVIGATION_STATE_OFFBOARD, NAVIGATION_STATE_MAX, diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1990651ef..1d23099f3 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -40,6 +40,9 @@ #include "esc.hpp" #include <systemlib/err.h> + +#define MOTOR_BIT(x) (1<<(x)) + UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), @@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - + for (unsigned i = 0; i < num_outputs; i++) { + if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely @@ -111,6 +113,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast<int>(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled)); + } else { + msg.cmd.push_back(static_cast<unsigned>(0)); } } @@ -121,9 +125,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) (void)_uavcan_pub_raw_cmd.broadcast(msg); } -void UavcanEscController::arm_esc(bool arm) +void UavcanEscController::arm_all_escs(bool arm) +{ + if (arm) { + _armed_mask = -1; + } else { + _armed_mask = 0; + } +} + +void UavcanEscController::arm_single_esc(int num, bool arm) { - _armed = arm; + if (arm) { + _armed_mask = MOTOR_BIT(num); + } else { + _armed_mask = 0; + } } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e6..12c035542 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -61,7 +61,8 @@ public: void update_outputs(float *outputs, unsigned num_outputs); - void arm_esc(bool arm); + void arm_all_escs(bool arm); + void arm_single_esc(int num, bool arm); private: /** @@ -99,6 +100,11 @@ private: uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer; /* + * ESC states + */ + uint32_t _armed_mask = 0; + + /* * Perf counters */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 8e6a9a22f..0d9ea08c5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; _scale.z_scale = 1.0F; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 9608ce680..0999938fc 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; } + // update device id as we now know our device node_id + _device_id.devid_s.address = static_cast<uint8_t>(node_id); + // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1316f7ecc..e31960537 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -112,6 +112,8 @@ protected: _channels(new Channel[MaxChannels]) { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; + _device_id.devid_s.bus = 0; } /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44..2c543462e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -279,6 +279,7 @@ int UavcanNode::run() _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -344,7 +345,13 @@ int UavcanNode::run() } // can we mix? - if (controls_updated && (_mixers != nullptr)) { + if (_test_in_progress) { + float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; + test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + + // Output to the bus + _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. @@ -384,15 +391,27 @@ int UavcanNode::run() } } - // Check arming state + // Check motor test state bool updated = false; + orb_check(_test_motor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); + + // Update the test status and check that we're not locked down + _test_in_progress = (_test_motor.value > 0); + _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); + } + + // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down - bool set_armed = _armed.armed && !_armed.lockdown; + // Update the armed status and check that we're not locked down and motor + // test is not running + bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } @@ -429,7 +448,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index be7db9741..274321f0d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -41,6 +41,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> +#include <uORB/topics/test_motor.h> #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -103,6 +104,10 @@ private: actuator_armed_s _armed; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus + int _test_motor_sub = -1; ///< uORB subscription of the test_motor status + test_motor_s _test_motor; + bool _test_in_progress = false; + unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer |